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ABSTRACT 

This  project  considers  a  ballistic  munition  shell  with  an  automated  flight  control  sys¬ 
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simulated  and  analyzed.  Several  shell  roll  rates  between  0  and  2Hz  are  tested  to  quan¬ 
tify  estimator  performance  across  the  specified  roll  rate  frequency  spectrum.  A  priori 
gravity  turn  information  is  substituted  for  GPS  measurements  to  make  an  inertial  only 
algorithm  and  the  performance  effects  are  quantified.  Finally,  an  extended  Kalman  fil¬ 
ter  implementation  of  the  inertial  only  roll  estimating  algorithm  is  developed  and  its 
performance  is  simulated  and  analyzed  by  first  isolating  the  individual  contributions  of 
each  sensor  error  and  then  considering  the  full  suite  of  errors. 
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Chapter  1 
Introduction 


The  improvements  over  the  past  decade  in  micro  inertial  sensor  technology  at  the 
Charles  Stark  Draper  Laboratory  have  made  feasible  the  development  of  micro  inertial 
navigation  systems.  Vehicles  whose  small  size  prohibited  installation  of  the  latest  sen¬ 
sor  technology  available  for  larger  platforms,  are  now  ripe  for  navigation  and  control 
enhancement.  The  ballistic  artillery  shell  is  such  a  platform  and  is  the  subject  of  this 
thesis. 

This  research  is  a  precursor  to  the  competent  munitions  program  at  Draper  Laboratory 
directed  by  the  Naval  Surface  Fire  Support  Programs  Office  and  called  the  Guided 
Munitions  Demonstrations  Program.  The  project  involves  development  of  a  Guidance 
and  Navigation  package  to  be  integrated  into  the  cylindrical  body  of  a  5  inch  diameter 
naval  gun  shell.  The  adapted  shell  is  fitted  with  canards  as  control  surfaces  and  is 
called  the  Guided  Munition  Round  (GMR,  see  Figure  1.1 ). 


FIGURE  1.1  Guided  Munition  Round 
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Draper  is  under  contract  to  build  five  Guidance  and  Navigation  boards  and  deliver 
them  to  the  Navy  for  firing  at  a  test  range.  Once  the  risk  reducing  Guided  Munition 
Demonstration  Program  is  complete,  a  follow-on  program  is  planned  to  integrate  the 
micro  sensors  and  GPS  based  guidance  system  into  the  smaller  nose  cone  section  of 
the  shell  [1]  . 

1.1  Guided  Munition  Round  Requirements 

To  this  date,  artillery  shell  targeting  systems  delivered  unguided  munitions  to  target  by 
using  a  priori  knowledge  of  shell  barrel  exit  velocity,  shell  aerodynamics,  and  wind 
estimates  to  aim  the  gun  barrel  in  azimuth  and  elevation  at  a  known  target  location. 
Years  of  perfecting  these  targeting  techniques  yield  a  Circular  Error  Probable  (CEP)  of 
about  330  feet  for  a  7.5  mile  shot  at  a  stationary  target.  This  means  that  50%  of  the 
shots  fired  will  land  within  330  feet  of  the  target  at  a  range  of  7.5  miles.  The  accuracy 
goal  for  the  GMR  is  50  feet  CEP  for  the  same  7.5  mile  shot.  In  addition  to  CEP  perfor¬ 
mance  of  the  overall  GN&C  system,  there  is  a  technology  demonstration  requirement 
for  the  electronics  to  simply  survive  the  launch  environment  of  6200g  (lasting  approx¬ 
imately  10ms)  and  operate  throughout  the  trajectory.  [1] 

1.2  Guided  Munition  Round  Time  Line 

The  Guided  Munition  system  concept  is  based  upon  a  series  of  events  that  follow  a 
specific  time  line  shown  in  Figure  1.2 .  The  GPS  receiver  will  be  allowed  to  acquire  all 
satellites  available  on  the  ground  prior  to  launch.  Fast  reacquisition  of  the  GPS  P-code 
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accuracy  signals  will  occur  during  the  flight.  P-code  accuracy  is  assumed  because  this 
is  a  military  application. 


t  =  0s  t=ls  t=15s 


Acquire  Load  Launch  Elec.  Recover  Reacquire  GPS 
QP5  from  Shock 


t  =  60s 

- -► 

Null  Miss  Distance  Impact 


FIGURE  1.2  GMR  Flight  Time  Line 

The  GMR  exits  the  barrel  of  the  gun  25ms  after  launch  and  spinning  at  approximately 
25Hz.  The  fins  and  canards  deploy  and  latch  within  30m  of  the  barrel  (or  about 
0.01  sec).  The  shell  subsequently  de-spins  to  within  0-2Hz  in  about  1  second.  The 
GPS/INS  is  inactive  for  the  first  second  of  the  flight  and  then  reacquisition  of  the  GPS 
signal  is  begun  along  with  the  local  vertical  estimation  routine  which  is  the  subject  of 
this  thesis.  GPS  reacquisition  is  complete  in  about  15  seconds  and  the  Guidance,  Nav¬ 
igation  and  Control  (GN&C)  system  makes  course  corrections  for  the  rest  of  the  tra¬ 
jectory  (about  45  seconds).  [1] 

1.3  Micro  Inertial  Navigation 

A  Micro  Inertial  Measurement  Unit  (MEMO)  consists  of  3  each  orthogonally  mounted 
micro  gyros  and  micro  accelerometers.  Cost  per  unit  of  a  MIMU  depends  on  the  accu¬ 
racy  required  of  the  gyros  and  accelerometers.  If  a  navigation  system  design  can  toler¬ 
ate  reasonable  system  errors,  then  gyro  and  accelerometer  manufacturing  is  relaxed 
resulting  in  relatively  inexpensive  MIMUs.  However,  if  the  navigation  system  requires 
greater  accuracies  from  the  noisy  micro  sensors  the  cost  per  unit  begins  to  climb. 
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A  typical  aircraft  inertial  navigation  system  consisting  of  3  each  orthogonally  mounted 
ring  laser  gyros  and  accelerometers  is  far  too  large  for  integration  into  the  guided 
munition  shell  platform.  The  most  accurate  gyros  and  accelerometers  are  not  only  too 
large  for  the  artillery  shell  application,  but  the  cost  per  unit  of  such  systems  is  too  high 
when  considering  the  “use  once  and  destroy”  mission  scenario  of  the  artillery  shell 
autopilot.  Finally,  the  larger  rate  gyros  are  not  sturdy  enough  to  withstand  the  high  g- 
loads  experienced  by  the  artillery  shell  platform. 

The  MIMU  accelerometer  cube  is  2”  x  2”  x  2”  for  the  Guided  Munitions  Demonstra¬ 
tion  Program.  The  hybrid  micro  gyros  require  3  boards  4”  x  4”  x  0.75”  each.  Total  vol¬ 
ume  for  the  MIMU  is  44  cubic  inches  (or  roughly  a  6.6”  cube).  The  MIMU  is  made  of 
relatively  light  electronic  components  so  the  total  weight  is  on  the  order  of  ounces. 
Electric  power  to  operate  the  MIMU  on  the  order  of  60  watts  and  the  MIMU  target 
cost  is  around  $5000.  [1] 

The  MIMU  micro  gyro  performance  requirements  are  described  in  terms  of  scale  fac¬ 
tor  error,  g  sensitivity,  misalignment,  non-orthogonality,  and  angle  random  walk.  The 
micro  accelerometer  performance  requirements  are  described  in  terms  of  scale  factor 

error.  g‘  sensitivity,  misalignment,  non-orthogonality,  and  velocity  random  walk. 
These  are  described  in  detail  in  Chapter  6  on  page  98. 

1.4  GPS  Navigation 

Embedding  a  GPS  receiver  in  the  artillery  shell  will  ease  the  accuracy  requirements  on 
the  MIMU.  The  ability  to  receive  GPS  signals  on  board  an  artillery  shell  and  tran- 
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spend  to  a  ground  station  has  been  demonstrated  in  Army  tests  and  documented  in 
GPS  World  and  other  papers  [1 1] ,  [2] .  Four  patch  antennas  will  be  mounted  at  equi¬ 
distant  locations  around  the  shell’s  cylindrical  body.  A  multi-channel  GPS  receiver  is 
embedded  to  provide  position  and  velocity  (initial  program)  or  psuedorange  and  delta 
range  (follow-on  program)  to  the  navigation  system  throughout  the  flight. 

1.5  The  Initialization  Problem 

The  g-load  on  the  sensors  at  launch  will  pin  the  micro  sensors  against  their  maximum 
deflection  points  and  effectively  corrupt  any  state  information  from  these  sensors. 
Once  out  of  the  barrel,  the  MIMU  and  GPS  receiver  should  operate  as  specified,  but 
the  initial  state  information  will  be  lost.  Before  a  navigation  system  can  begin  to  feed 
state  information  into  a  control  algorithm  to  guide  the  shell,  an  estimate  of  the  initial 
state  starting  at  some  time  after  leaving  the  barrel  must  be  made  to  initialize  the  trans¬ 
formation  from  body  to  local  level  coordinate  frames.  This  initial  state  estimate  must 
be  provided  with  enough  time  remaining  in  the  flight  for  the  initialized  navigation  rou¬ 
tine  and  control  system  to  correct  the  shell  trajectory. 

The  a  priori  knowledge  used  to  target  the  shell  in  the  absence  of  GN&C  becomes  use¬ 
ful  in  this  state  initialization  problem  [4] .  The  azimuth  and  elevation  of  the  barrel 
along  with  a  cmde  estimate  of  a  gravity  turning  ballistic  trajectory  can  provide  decent 
initial  state  estimates  of  the  shell  pitch  and  yaw  angles.  These  angles,  knowledge  of 
the  shell  exit  velocity  (known  to  a  high  degree  of  accuracy  from  years  of  Army  and 
Navy  testing),  and  the  location  of  the  gun  barrel  can  yield  reasonable  initial  estimates 
of  shell  position  and  velocity.  However,  determining  the  shell  roll  angle  defies  such  an 
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easy  solution  and  is  required  to  find  the  local  vertical  state.  The  local  vertical  state  is 
defined  as  a  body  centered  coordinate  frame  with  the  “z”  axis  always  pointing  down 
toward  the  center  of  the  earth.  Without  knowledge  of  the  shell  attitude  within  the  local 
vertical  state,  any  control  surface  deflections  commanded  to  the  canards  attached  to 
the  surface  of  the  spinning  GMR  would  result  in  erroneous  course  corrections. 

However,  ample  information  is  available  from  the  installed  sensors  to  estimate  the 
shell  roll  angle  to  the  required  degree  of  accuracy.  It  is  estimated  that  an  initial  error  of 
30  degrees  reducing  to  about  6  degrees  of  error  at  the  end  of  the  flight  will  allow  the 
GN&C  to  meet  the  50  ft.  CEP  performance  goal  [4] .  The  research  presented  here 
focuses  on  development  of  a  local  vertical  estimation  algorithm  and  analysis  of  its  per¬ 
formance  in  the  presence  of  various  error  sources. 
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Chapter  2 


GPS  Velocity  Measurements  to  Estimate 
the  Pitch  and  Yaw  Rates  of  a  Non- 
Spinning  Shell 

Before  proceeding  with  the  shell  roll  angle  estimation  problem,  an  acceptable  estimate 
of  the  shells  pitch  and  yaw  rates  needs  to  be  produced.  This  is  foundational  to  the  latter 
sections  because  it  will  be  shown  that  combining  lateral  rate  gyro  data  (y  and  z  axis 
rate  gyros  in  the  body  fixed  frame)  with  these  estimates  of  shell  pitch  rate  and  yaw  rate 
yields  a  measurement  of  the  shell  roll  angle.  With  this  motivation  in  mind,  this  chapter 
endeavors  to  use  information  from  GPS  velocity  data  (which  is  available  once  per  sec¬ 
ond)  to  estimate  the  shell  pitch  and  yaw  rate  in  real  time  during  the  trajectory. 

2,1  A  Look  at  the  Non-Spinning  Trajectory 

A  ballistic  trajectory  of  a  non-spinning  artillery  shell  was  modeled  for  the  analysis  in 
this  chapter.  Two  independent  software  simulations  were  developed  to  model  the  artil¬ 
lery  shell  navigation  environment.  The  first  simulation  generates  a  trajectory  file  of 
shell  position,  velocity,  acceleration,  specific  force,  attitude  quaternion,  angular  rate 
and  angular  acceleration  at  a  lOOOHz  data  rate.  The  second  simulation  incorporates 
models  of  GPS  and  the  MIMU  with  error  sources,  and  navigation  algorithms  to  simu¬ 
late  navigation  system  performance.  The  lOOOHz  sensor  information  rate  was  chosen 
for  this  thesis  to  demonstrate  performance  when  the  shell  spin  rate  is  2Hz.  A  complete 
description  of  this  simulation  is  given  in  Chapter  6  .  The  trajectory  simulated  here  has 
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a  0  degree  azimuth  angle  (due  north)  and  a  38.75  degree  elevation  angle.  The  initial 
velocity  out  of  the  barrel  is  830  m/s.  The  shell  position  in  North  East  Down  (NED) 
coordinate  frame  is  given  in  Figure  2. 1  . 


30  40  50 
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FIGURE  2.1  Non-Spinning  Artillery  Shell  Position  in  NED 
Notice  the  left  turn  the  shell  takes  due  to  the  Coriolis  effect  on  the  moving  shell  in  a 
rotating  coordinate  frame.  The  non-spinning  shell  attitude  under  these  conditions  is 
given  in  Figure  2.2  . 
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FIGURE  2.2  Artillery  Shell  Attitude  for  a  Non-Spinning  Trajectory 
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The  ballistic  trajectory  shows  the  artillery  shell  pitching  down  throughout  the  flight  as 
well  as  yawing  to  the  left.  Since  the  pitch  rate  is  always  acting  in  the  negative  “East” 
direction  in  NED  and  the  Yaw  Rate  always  acts  in  the  negative  “Down”  direction  in 
NED,  these  rates  both  contain  information  about  the  local  level  direction.  The  Pitch 
and  Yaw  rates  for  the  non-spinning  trajectory  are  given  in  Figure  2.3  . 


FIGURE  2.3  Non-Spinning  Pitch  and  Yaw  Rates 
This  information  will  be  exploited  by  the  initialization  algorithm  to  estimate  the  shell 
roll  angle  when  the  shell  is  spinning. 


2.2  Velocity  Estimation  at  lOOOHz  from  GPS  Velocity 
Measurements  at  IHz 

As  will  be  shown  later,  the  GPS  velocity  data  contains  information  about  the  non-spin¬ 
ning  pitch  and  yaw  attitudes;  however,  the  GPS  update  rate  is  only  IHz.  The  velocity 
profile  of  the  shell  shown  in  Figure  2.4  shows  the  changes  in  velocity  due  to  aerody- 
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namic  drag  and  gravity.  Early  in  the  trajectory,  a  one  second  time  interval  can  see  as 
much  as  50  m/s  velocity  change.  Clearly,  a  one  second  time  step  is  too  large  to  assume 
velocity  is  merely  constant.  Therefore,  linear  least  squares  data  interpolation  is  used  to 
provide  the  velocity  estimate  across  the  time  step. 
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FIGURE  2.4  Shell  Velocity  Profile  in  NED 

Third  order  linear  least  squares  curve  fitting  was  implemented  into  the  software  simu¬ 
lation  to  provide  a  3rd  order  polynomial  estimate  of  the  velocity  profile  between  the 
one  second  GPS  velocity  updates.  Nth  order  least  squares  curve  fitting  is  a  technique 
to  find  the  Nth  order  polynomial  that  best  fits  a  set  (or  “batch”)  of  data  collected  over  a 
certain  period  of  time.  A  complete  description  of  this  technique  can  be  found  in  refer¬ 
ence  [8] .  Six  consecutive  GPS  velocity  measurements  were  collected  sequentially  to 
create  the  least  squares  3rd  order  polynomial  best  fitting  the  6  GPS  data  points.  After 
the  initial  6  seconds  to  fill  the  batch  processing  register,  each  additional  second 
included  a  new  GPS  velocity  measurement,  eliminated  the  oldest  GPS  velocity  mea- 
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surement  in  the  register,  and  recalculated  the  3rd  order  polynomial  coefficients  using 
the  latest  6  data  points.  During  the  1  second  intervals  between  GPS  measurements, 
velocity  is  estimated  using  time  and  the  3rd  order  least  squares  polynomial  coeffi¬ 
cients.  The  results  of  this  method  in  the  absence  of  sensor  errors  is  given  below  in  Fig¬ 
ure  2.5  . 


FIGURE  2.5  Velocity  Error  Using  Srd  Order  Least  Squares  Curve  Fit  (No  other  errors) 
The  “saw  tooth”  effect  is  due  to  the  error  between  truth  and  the  3rd  order  polynomial 
curve  fit  provided  by  least  squares  estimation.  The  errors  level  out  in  time  revealing 
that  the  3rd  order  polynomial  curve  fit  is  more  accurate  as  the  velocity  profile  smooths 
out  with  time  along  the  80  second  trajectory.  However,  GPS  velocity  measurements 
are  not  without  errors.  Noise  was  added  (a  =  0. 1  m/s)  to  the  GPS  data  and  the  resulting 
error  is  shown  in  Figure  2.6  . 
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FIGURE  2.6  Velocity  Error  Using  3rd  Order  Least  Squares  Curve  Fit  and 
GPS  Noise  at  a  =  0.1  m/s 

2.3  GPS  Euler  Angle  Projection 

Now  that  a  reliable  velocity  estimate  is  available  at  the  system  rate  of  lOOOHz,  this 
velocity  measurement  can  be  used  to  determine  the  non-spinning  shell  pitch  and  yaw 
attitude.  First,  an  assumption  is  made  to  simplify  the  analysis.  It  is  assumed  that  the 
roll  (or  “x”)  axis  in  the  body  frame  is  aligned  with  the  velocity  vector.  In  reality,  artil¬ 
lery  shells  usually  achieve  some  angle  of  attack.  Implementation  of  the  following  tech¬ 
nique  will  require  additional  complexity  to  adjust  for  angle  of  attack.  With  this 
assumption  made,  the  velocity  vector  components  in  the  NED  frame  are  related  to  the 
shell  attitude  as  illustrated  in  Figure  2.7  . 


28 


North 


FIGURE  2.7  GPS  Euler  Angle  Projection  in  North  East  Down  Coordinates 
The  equations  relating  velocity  to  yaw  and  pitch  angle  are  as  follows.  The  terms  \|/,  and 
6  are  “hatted”  to  denote  that  they  are  estimates. 

tan<i/  =  ^  EQ  (2.3.1) 


EQ  (2.3.2) 


FIGURE  2.8  GPS  Euler  Angle  ProjectOe  Error 


In  the  absence  of  errors,  the  error  in  estimated  Euler  angles  using  this  technique  are 
presented  above  in  Figure  2.8  .  The  results  with  GPS  velocity  noise  set  at  c  =  0.1  m/s 
is  given  in  Figure  2.9  . 


FIGURE  2.9  GPS  Euler  Angle  Projection  Error  with  Velocity  Noise  a  =  O.lm/s 

2.4  Estimating  Pitch  and  Yaw  Rates  of  the  Non-Spinning 
Shell 

Pitch  and  yaw  rates  are  obtained  by  dividing  the  estimated  pitch  and  yaw  angles  by  the 
time  step  determined  by  the  data  processing  rate.  The  equations  for  estimated  pitch 
and  yaw  rate  (Q  and  R  respectively)  are  as  follows. 

X  A0 

(2  =  —  EQ  (2.4.1) 

f.  Mf 

R  =  -^  EQ  (2.4.2) 

At 

Estimates  of  Q  and  R  can  be  made  by  differentiating  (across  the  lOOOHz  time  step)  the 
change  in  estimated  yaw  and  pitch  angles  estimated  in  turn  by  GPS  Euler  angel  projec- 
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tion.  The  results  of  this  technique  are  presented  in  Figure  2.10  without  GPS  velocity 
noise,  and  in  Figure  2.11  with  the  GPS  velocity  noise  of  a  =  0. 1  m/s. 


FIGURE  2.10  Non-Spinning  Pitch  and  Yaw  Rate  Estimates  (No  Noise) 
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FIGURE  2.11  Non-Spinning  Pitch  and  Yaw  Rate  Estimates  (deg/sec)  with 
GPS  Velocity  Noise  a  =  0.1  m/s 

The  spikes  that  appear  periodically  in  Figure  2.10  and  Figure  2.11  are  peculiarities 
due  to  the  software  implementation  of  this  routine.  They  are  inconsequential  to  the 
performance  of  the  navigation  algorithm  developed  in  later  sections  because  these 
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algorithms  will  be  insensitive  to  the  pitch  and  yaw  rate  magnitudes  provided  by  GPS 
Euler  Angle  Projection. 


32 


Chapter  3 

Estimating  the  Roll  Angle  of  a  Spinning 
Shell 

This  chapter  focuses  on  estimating  the  roll  angle  of  a  shell  spinning  at  2Hz.  The  prob¬ 
lem  must  be  solved  without  prior  knowledge  of  the  shells  initial  roll  angle.  The  chap¬ 
ter  begins  by  showing  that  the  lateral  rate  gyros  (or  the  y  and  z  axis  rate  gyros  mounted 
in  the  body  frame  of  the  spinning  shell)  can  be  combined  to  provide  measurements  of 
the  sine  and  cosine  of  the  shell  roll  angle.  However,  the  gyros  are  very  noisy.  The  next 
portion  of  the  chapter  is  devoted  to  deriving  a  least  squares  estimator  that  can  extract 
the  sine  and  cosine  of  roll  from  the  noisy  measurements.  This  estimator  employs  some 
trigonometric  identities  to  relate  the  measurements  to  a  linear  combination  of  known 
and  unknown  quantities.  The  known  quantity  turns  out  to  be  the  sine  and  cosine  of  the 
roll  rate  multiplied  by  time  and  the  unknown  quantity  is  the  sine  and  cosine  of  the 
phase  of  the  roll  angle.  Since  the  relationship  between  measurements  and  unknowns  is 
made  linear,  a  deterministic  least  squares  curve  fitting  approach  is  applied  to  estimate 
the  unknown  quantities.  With  this  done,  the  roll  angle  can  easily  be  found  by  applying 
the  arctangent  function.  The  last  part  of  the  chapter  is  intended  to  analyze  the  perfor¬ 
mance  of  this  method  in  the  presence  of  a  variety  of  rate  gyro  errors. 

To  start,  it  is  assumed  that  the  spinning  shell  follows  the  same  trajectory  as  the  non¬ 
spinning  shell  for  shell  roll  rates  ranging  from  OHz  to  2Hz.  If  atmospheric  effects  other 
than  drag  were  considered  in  the  trajectory  model,  a  spinning  shell  encounters  an  addi- 
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tional  yawing  moment  due  to  the  interaction  between  the  air  stream  and  the  spinning 
boundary  layer  [5] .  The  effect  is  called  the  yaw  of  repose  and  is  ignored  for  this  anal¬ 
ysis  because  it  is  negligible  at  the  small  roll  rates  being  considered  [3] . 

3.1  Roll  Information  Imbedded  in  Pitch  and  Yaw  Axis  Rate 
Gyros 

As  spin  about  the  shell  nose  is  introduced  into  the  trajectory,  the  body  frame  rotates  in 
relation  to  the  non-spinning  reference  frame  at  the  roll  rate.  The  pitch  and  yaw  rate 
gyros  mounted  in  the  spinning  body  axis  will  detect  the  non-spinning  pitch  and  yaw 
rates  once  per  revolution  when  the  roll  angle  is  identically  zero.  As  the  shell  rolls 
through  360  degrees  the  angular  rate  measured  by  each  will  vary  sinusoidally  with  a 
period  equal  to  the  period  of  the  shell  spin.  This  is  illustrated  below  in  Figure  3.1  . 


when  Roll  =  90  deg... 

(oJ’  =  Ro\l  RatCsheii 
cOy  =  Yaw  RatCj^Qj^^gpjj^ 

cOz  =  -  Pitch  R^tCjjQj^.gpjj^ 


FIGURE  3.1  Angular  Rates  in  the  Rotating  Body  Frame  Compared  to  a  Non-Spinning  Ref. 

Frame 
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The  angular  rates  sensed  by  the  rate  gyros  mounted  in  the  y  and  z  body  axes  are  given 
by  the  following  formulas.  Here  (])  is  the  roll  angle,  and  Q  and  R  represent  the  pitch 
and  yaw  rate  respectively  in  the  non-spinning  reference  frame. 

(Oy^=  ecos(j)  +  Rsin(l)  EQ  (3.1.1) 

/?cos(j)  -  !2sin(j)  EQ  (3.1.2) 

In  the  absence  of  errors  from  the  rate  gyros,  the  detected  y  and  z  angular  rates  in  the 
body  frame  with  the  spin  rate  P  =  1/4  Hz  are  shown  in  Figure  3.2  . 


Time  in  seconds 


FIGURE  3.2  Spinning  Shell  Angular  Rates  in  Body  Axes 

3.2  Roll  Estimation  Using  Linear  Least  Squares 

Solving  EQ  3.1.1  and  EQ  3.1.2  for  sincj)  and  coscj)  (where  (j)  is  time  varying)  produces 
the  following  relationships. 
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EQ  (3.2.1) 


<2o)/  +  /?(o/ 

COS(l)(f)  =  - - - - 

2  ^ 

Q  +R 

R%  -  <2®/ 

sin(t)(0  =  — - — 

Q  +R 


EQ  (3.2.2) 


Combining  the  lateral  rate  gyro  measurements  and  using  the  estimates  of  R  and  Q,  as 
given  in  Section  2.4,  produces  processed  measurements  of  cos(l)(t)  and  sin<|)(t).  Initial 
attempts  at  extracting  the  roll  angle  information  from  these  equations  are  documented 
in  the  Appendices  of  this  report.  The  linear  least  squares  algorithm  presented  here 
works  relatively  well  and  will  give  insight  into  the  Kalman  Filter  approach  presented 
later.  Focusing  our  attention  on  EQ  3.2.2  as  the  measurement  of  sin(})(t),  the  sensor 
data  combined  in  this  way  produces  a  measurement  vector  at  the  system  rate  of 
lOOOHz.  Figure  3.3  shows  the  sin(])(t)  measurement  for  a  spinning  trajectory  of  1/4  Hz 
with  gyro  noise  as  specified  in  Chapter  6.  The  sin(t)(t)  measurements  extend  outside  the 
-1  to  1  range  of  a  pure  sinusoid  because  of  the  noise  in  the  rate  gyro  outputs  which  are 
sampled  at  the  system  rate  of  lOOOHz.  The  variance  of  the  rate  gyro  noise  is  also 
affected  by  the  “Q”  and  “R”  terms  in  EQ  3.1.2.  This  relationship  is  derived  in 


Section  5. 2. 1.1. 


Sin(t)(t)  can  be  written  two  ways  as  shown  in  EQ  3.2.3  and  EQ  3.2.4  where  <E>p  is  the 
phase  of  the  sin(t)(t)  curve.  Notice  that  EQ  3.2.4  is  linear  in  cos<I>p  and  sinOp 


y(t)  =  sin[o)/-f  +  <I>pJ  EQ  (3.2.3) 

y(t)  =  sin[^C0j|.*  •  tj  •  cos  Op  +  cos[^co^^  •  tj  ■  sin  Op  EQ  (3.2.4) 

The  vector  notation  for  n  measurements  of  y(t)=  sin(t)(t)  (from  EQ  3.1.2  )  and  the 
matrix  form  of  EQ  3.2.4  at  discrete  times  is  provided  in  EQ  3.2.5  .  The  equation  is 
written  symbolically  in  EQ  3.2.6  .  The  roll  axis  rate  gyro  provides  the  Maximum  Like¬ 
lihood  (ML)  estimate  of  roll  rate  which  changes  with  time  throughout  the  trajec¬ 
tory  depending  on  roll  axis  rate  gyro  errors.  ML  estimation  of  angular  rate  from  noisy 
gyros  is  developed  in  Appendix  B  . 
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EQ  (3.2.5) 


EQ  (3.2.6) 


yj  is  the  measurement  defined  in  EQ  3.1.2  . 

is  the  ML  estimate  of  roll  rate  at  time  q  from  the  x  axis  gyro 
is  the  phase  as  defined  in  EQ  3.2.3 

Deterministic  linear  least  squares  curve  fitting  provides  the  minimum  error  estimate  of 
a  based  on  the  observations  y  and  the  calculated  matrix  relationship  Z.  This  estimate 


is  the  solution  to  the  normal  equations  for  this  system  shown  in  EQ  3.2.7  .  [8] 
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EQ  (3.2.7) 


Once  the  estimation  of  the  phase  parameter  for  the  sintj)  signal  is  accomplished,  we  can 
extract  the  roll  angle  using  the  ML  estimate  of  spin  rate  (cOx^)  and  the  following  steps. 

sin$  (r)  =  sin(^d)/  •  t  +  <E)p  j  EQ  (3.2.8) 

cos$(0  =  cos[^d)/r+<E)pJ  EQ  (3.2.9) 

$  (0  sinit)  =  atanf 

^  COS$  (0  ^ 

The  entire  procedure  can  be  repeated  using  the  cos(j)  information  from  EQ  3.2.1  . 

^  ^  b 

COS<j)(0  =  - ^ ^  EQ  (3.2.11) 

2  ^ 

Q  +R 

The  LLSE  algorithm  is  repeated  exactly  as  shown  above  except  the  matrix  formulation 
for  y,  Z  and  a  come  from  the  cosine  expansion  shown  in  EQ  3.2.13  . 

y(0  =  cos(^©/ •  r  +  a)pj  EQ(3.2.12) 

y  (t)  =  cos[^©/  -  rj  ■  cosOp  -  sin(^  (oj"  ■  rj  •  sinOp  EQ  (3.2.13) 

The  a\  erage  of  the  two  roll  angel  estimates  represents  the  linear  least  squares  estimate 
of  roll  from  this  algorithm. 


^  LLSE  - 


^(0  sin(|)  COS(|) 

2 


EQ  (3.2.14) 


3.2.1  Effect  of  Micro  Gyro  and  GPS  Noise  on  the  LLSE  Algorithm 

The  LLSE  algorithm  error  for  a  simulated  trajectory,  gyro  noise  active,  and  P  =  2  Hz 
(batch  processing  time  At  =  10  sec  at  lOOOHz  data  rate)  is  shown  in  Figure  3.4  . 
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Clearly,  the  longer  the  batch  processing  time  the  better  the  algorithm  performance 
because  the  filter  gets  more  data  over  a  larger  number  of  cycles  to  estimate  phase. 
However,  not  much  time  is  available  because  the  shell  trajectory  only  lasts  80  seconds. 
The  batch  processing  time  of  10  seconds  is  used  throughout  this  report  as  a  benchmark 
for  analysis  and  a  seemingly  reasonable  time  in  considering  the  entire  system 


FIGURE  3.4  Roll  Error  from  ELSE  Algorithm  P=  2  Hz 
(Data  at  lOOOHz  and  At  =  10  sec) 

3.2.2  The  Effect  of  Scale  Factor  Error  on  the  ELSE  Algorithm 

Gyro  scale  factor  error  produces  errant  rate  indications  about  all  three  body  axis.  Such 
error  in  the  y  and  z  body  axis  rate  gyros  does  not  affect  the  algorithm  performance 
because  the  angular  rates  are  small.  Figure  3.5  shows  the  algorithm  performance  with 
gyro  noise,  GPS  noise  and  y  and  z  axis  rate  gyro  scale  factor  errors  active.  The  data  in 
the  first  17  seconds  are  aberrations  of  the  software  implementation  since  the  algorithm 
was  not  initialized  until  17  seconds  into  the  simulated  trajectory. 
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Time  in  seconds 

FIGURE  3.5  ELSE  Algorithm  Error  with  Y  and  Z  axis  Scale  Factor  Active 
Scale  factor  error  in  the  x  body  axis  rate  gyro  has  a  significant  effect  on  algorithm  per¬ 
formance.  An  attempt  to  estimate  and  remove  x  axis  scale  factor  error  will  be 
addressed  in  a  later  section.  Figure  3.6  shows  the  algorithm  performance  with  gyro 
noise,  GPS  noise  and  x  axis  rate  gyro  scale  factor  error  active. 


Time  In  seconds 

FIGURE  3.6  ELSE  Algorithm  Error  with  X  Axis  Scale  Factor  Active 
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Notice  that  the  average  error  is  about  14  degrees.  The  2000ppm  x  axis  gyro  scale  fac¬ 
tor  error  modelled  in  the  simulation  produces  a  1.44  deg/sec  roll  rate  error  (0.004Hz) 
when  the  spin  rate  P  =  2Hz.  The  average  error  is  not  surprisingly... 

3.2.3  The  Effect  of  Misalignment  and  Non-Orthogonality  on  the 
ELSE  Algorithm 

Misalignment  and  Non-Orthogonality  errors  have  little  effect  on  the  algorithm  perfor¬ 
mance  when  the  misalignment  of  the  y  or  z  body  axis  rate  gyros  is  solely  in  the  y-z 
plane  in  the  body  axis.  This  is  because  the  algorithm  uses  the  change  in  amplitude 
much  more  than  the  actual  value  of  that  amplitude  from  the  y  and  z  axis  rate  gyros. 
However,  whenever  the  misalignment  of  the  y  and  z  axis  rate  gyros  permits  some 
component  of  the  shell  spin  rate  to  be  detected  by  y  or  z  axis  gyros,  a  large  bias  is 
placed  on  the  gyro  data.  To  illustrate  this,  the  simulation  was  run  with  gyro  noise,  GPS 
velocity  noise,  and  gyro  non-orthogonality  active.  In  Figure  3.7  and  Figure  3.8  ,  the  x 
body  axis  was  tilted  0.096  rad  in  the  x-y  plane  and  the  y  body  axis  gyro  was  tilted 
0.096  rad  into  the  x-y  plane.  Notice  the  bias  in  the  y  or  “pitch”  measured  angular  rate 
in  Figure  3.7  .  About  70  deg/sec  of  the  720  deg/sec  spin  rate  is  detected  by  the  y  body 
axis  rate  gyro.  The  x  body  axis  detected  angular  rate  error  is  harder  to  notice  in  Figure 
3.7  but  still  significant.  When  this  bias  error  and  the  incorrect  shell  spin  rate  estimate 
are  introduced  into  the  LLSE  filter  the  error  in  roll  estimate  is  quite  large  as  shown  in 
Figure  3.8  . 
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FIGURE  3.7  Measured  Angular  Rates  in  Body  Axis  with  Non-Orthogonality  of  0.096  rad  in 
both  X  and  Y  Body  Axis  Rate  Gyros  in  the  X-Y  Plane 


FIGURE  3.8  Roll  Error  with  Non-Orthogonality  of  0.096  rad  in  both  X  and  Y  Body  Axis  Rate 

Gyros  in  the  X-Y  Plane 

In  the  next  run,  the  z  body  axis  was  tilted  0.096  rad  in  the  x-z  plane  in  addition  to  the 
previous  non-orthogonalities  to  indicate  the  decay  in  performance  when  both  the  y  and 
z  axis  rate  gyros  detect  a  component  of  the  shell  spin  rate.  Figure  3.9  shows  this  effect 
on  the  algorithm  roll  estimation  error  when  bias  error  from  both  the  y  and  z  axis  rate 
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gyros  as  well  as  the  incorrect  shell  spin  rate  estimate  are  introduced  into  the  LLSE  fil¬ 


ter. 


Time  in  seconds 

FIGURE  3.9  Roll  Error  with  Non-Orthogonality  of  0.096  rad  in  X,  Y  and  Z  Body  Axis  Rate 

Gyros 

Because  the  bias  error  is  a  constant  and  the  oscillatory  y  and  z  body  axis  rate  gyro  sig¬ 
nal  should  be  unbiased,  estimating  and  removing  this  error  is  easily  done  by  iteratively 
calculating  the  mean  of  the  y  and  z  body  axis  rate  gyros  over  the  batch  processing  time 
and  subtracting  this  value  from  the  subsequent  rate  gyro  measured  angular  rates.  This 
eliminates  a  significant  portion  of  Misalignment  and  Non-Orthogonality  error,  leaving 
the  incorrect  shell  spin  rate  error.  The  performance  of  the  Y  and  Z  axis  bias  error  cor¬ 
rection  is  shown  below  in  Figure  3.10 . 
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FIGURE  3.10  Roll  Error  with  Non-Orthogonality  of  0.096  rad  in  both  X  and  Y  Body  Axis 
Rate  Gyro  in  the  X-Y  Plane  with  Bias  Error  Correction  Implemented 

The  best  way  to  eliminate  non-orthogonality  error  and  MIMU  misalignment  error  of 

the  X  axis  rate  gyro,  is  to  calibrate  the  sensor  carefully  upon  MIMU  packaging  and 

installation  into  the  shell.  This  will  insure  the  x  axis  rate  gyro  is  lined  up  with  the 

actual  shell  roll  axis  to  a  much  better  accuracy  than  the  0.096  rad  (5.5  deg)  used  in  this 

analysis.  This  correction  will  greatly  reduce  this  error  at  roll  rates  of  2Hz  or  less. 

3.2.4  Estimating  Error  in  the  X  Body  Axis  Rate  Gyro  with  the  EFT 

Combining  the  y  and  z  axis  rate  gyro  data  as  set  up  in  Section  3.2  can  provide  a  direct 

measurement  of  either  sine])  or  cos(])  (sintj)  reshown  below  from  Figure  3.3  ). 
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FIGURE  3.11  Sin(})  from  y  and  z  Axis  Rate  Gyros 
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Figure  3.11  indicates  that  the  shell  roll  rate  is  the  dominant  frequency  in  this  signal. 
An  attempt  was  made  to  use  the  sin({)  measurements  from  the  shell  simulation  with 
2Hz  spin  rate  (data  at  lOOOHz  and  micro  gyros  with  noise  as  currently  specified)  to 
provide  the  roll  rate  of  the  shell  independent  of  the  x  axis  rate  gyro. 

3.2.4.1  N  Dimensional  Discrete  Fourier  IVansform  Description 

An  N  Dimensional  Discrete  Fourier  Transform  (NDFT)  is  able  to  identify  relative  con¬ 
tributions  of  N  discrete  frequency  components  of  a  signal  by  utilizing  orthogonality  to 
separate  a  sampled  function  into  individual  contributions  of  each  frequency.  These  fre¬ 
quency  components  are  equally  spaced  between  +  and  -  the  value  of  the  sampling  fre¬ 
quency  divided  by  2  (or  fs/2  where  f^  =  lOOOHz  and  so  the  frequencies  are  equally 
spaced  between  -500Hz  and  +500Hz).  If  the  amount  of  sampled  data  is  smaller  than  N 
(the  number  of  frequencies  components),  the  sampled  data  can  be  “zero  padded”  to  fill 
the  observation  vector  with  O’s  until  it’s  size  “n”  is  equal  to  N.  A  special  form  of  the 
DFT  (or  algorithm  to  compute  the  DFT)  is  called  the  Fast  Fourier  Transform  (FFT) 
and  is  especially  useful  in  computer  applications  because  of  a  reduced  amount  of  cal¬ 
culations  to  provide  the  same  results  as  the  DFT.  A  complete  discussion  of  the  DFT 
and  FFT  is  contained  in  reference  [8] . 

3.2.4.2  NFFT  Performance  as  Roll  Rate  Estimator 

Since  the  error  we  are  trying  to  detect  in  the  2Hz  roll  rate  is  in  the  range  of  1.44  deg/ 
sec  (0.004Hz),  an  NFFT  will  have  to  provide  resolution  greater  than  this  to  be  of  use. 
With  a  sampling  frequency  fg  =  lOOOHz,  an  N  =  1,500,000  point  NFFT  is  used  to  pro¬ 
vide  discrete  frequency  component  every  0.0006667Hz.  Sampling  sinrj)  data  for  10 
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seconds  at  lOOOHz  produces  a  function  vector  F  with  10,000  points.  10  seconds  was 
chosen  as  the  batch  processing  time  to  match  the  LLSE  fast  spinning  algorithm  At. 
This  huge  FFT  requires  1,490,000  zeros  to  pad  the  function  vector  F!  A  Matlab  routine 
was  created  to  perform  the  NFFT  on  data  from  the  simulated  30km  shell  flight  (79  sec¬ 
onds).  The  NFFT  from  one  of  the  10  second  periods  is  shown  below  (scaled  to  the  fre¬ 
quencies  of  interest). 


FIGURE  3.12  1,500,000  Point  NFFT  of  10s  Sm(l)  Data  Sampled  at  lOOOHz 
Extracting  the  maximum  Fourier  coefficient  from  this  data  placed  the  shell  roll  rate  at 
1.993Hz.  The  error  in  this  particular  estimate  is  0.007Hz  and  is  worse  than  the  x  axis 
rate  gyro  with  scale  factor  error  included.  The  results  from  the  other  10  second  periods 
of  sampled  data  were  similar  in  accuracy.  The  failure  of  the  NFFT  to  predict  the  shell 
spin  rate  within  required  accuracy  as  demonstrated  in  this  simulation,  plus  the  need  for 
a  great  deal  of  storage  and  processing  capability  in  real  time  during  the  shell  flight 
makes  this  approach  undesirable.  The  performance  of  the  NFFT  at  predicting  roll  rate 
is  certain  to  get  worse  in  the  real  environment  where  the  roll  rate  is  not  exactly  con¬ 
stant  over  the  batch  processing  time  period. 
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3.2.4.3  Estimating  X  Axis  Gyro  Scale  Factor  Error  in  the  Navigation  Filter 
Another  way  must  be  found  to  estimate  and  remove  the  effects  of  scale  factor  error  in 
the  X  axis  gyro.  Additional  information  from  GPS  position  and  velocity  measurements 
as  well  as  accelerometer  information  from  the  MIMU  could  be  combined  to  provided 
an  estimate  of  scale  factor  error.  Remember  that  the  final  desired  result  of  the  Guided 
Munitions  Demonstration  Program  is  the  creation  of  a  complete  navigation  filter  to 
estimate  position,  velocity  and  angular  state.  Current  research  [4]  indicates  this  filter 
has  the  abihty  (once  GPS  is  acquired)  to  estimate  x  axis  scale  factor  error  pretty  well 
by  utilizing  information  from  the  MIMU  and  GPS  position.  Prior  to  GPS  acquisition, 
there  is  no  other  information  available  to  estimate  scale  factor  error.  Since  most  of  the 
control  corrections  will  occur  in  the  last  half  of  the  shell  trajectory,  14  deg  of  error  as 
an  initial  value  is  not  too  bad  if  the  information  to  remove  this  error  is  available  later  in 
the  flight. 

3.3  The  Fast  Spinning  Shell  Algorithm  without  GPS 

It  would  be  nice  not  to  wait  for  GPS  data  to  begin  the  fast  spinning  algorithm  to  esti¬ 
mate  the  roll  angle.  This  is  true  for  a  couple  of  reasons.  First,  remember  the  GMR 
time-line  in  Figure  1.2  which  shows  GPS  will  not  be  available  until  about  15-20  sec¬ 
onds  into  the  flight  of  the  shell.  This  does  not  leave  much  time  for  course  corrections 
which  cannot  begin  without  some  initial  estimate  of  the  local  vertical  state.  If  GPS  was 
not  necessary  to  estimate  the  local  vertical  state,  then  the  time  constraint  is  relaxed. 
The  addition  of  GPS  measurements  at  20  seconds  will  simply  improve  the  navigation 
solution.  Second,  the  ballistic  artillery  shell  could  conceivably  encounter  jamming  of 
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its  GPS  receiver  and  lose  access  to  GPS  information.  If  this  were  to  occur,  indepen¬ 
dence  from  GPS  measurements  would  allow  the  local  vertical  estimate  to  operate 
through  the  jamming. 

Until  now,  the  fast  spinning  roll  estimating  routine  has  used  GPS  velocity  measure¬ 
ments  to  estimate  the  shell  pitch  over  (gravity  turn)  rate  “Q”  and  the  shell  yaw  rate  due 
to  Coriolis  and  repose  forces  “R.”  Since  the  azimuth  and  elevation  of  the  gun  barrel  as 
well  as  the  range  of  the  trajectory  are  known  a  priori,  good  estimates  of  these  values 
are  available  prior  to  GPS  estimates  of  them.  A  crude  demonstration  yields  some 
enlightening  results.  Let  Q  =  -1  deg/sec  (constant)  and  R  =  -0.01  deg/sec  (constant) 
until  GPS  is  available  27  seconds  into  the  trajectory.  Figure  3.13  shows  the  resulting  Q 
and  R  estimates  for  the  simulated  flight. 


Tim©  in  seconds 


FIGURE  3.13  Q  and  R  Estimates  with  No  GPS  for  27s 
With  the  shell  spin  rate  at  2Hz,  the  fast  spinning  algorithm  performance  in  Figure  3.14 
reveals  no  discernible  improvement  when  GPS  data  becomes  available. 
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FIGURE  3.14  Roll  Estimating  Error  No  GPS  for  27s 
What  would  the  roll  estimation  error  look  like  without  GPS  information  for  the  entire 
flight?  The  constant  Q  and  R  estimates  are  shown  in  Figure  3.15  and  the  resulting  roll 
estimation  error  in  shown  in  Figure  3.16  . 


FIGURE  3.15  Q  and  R  Estimates  with  No  GPS 
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FIGURE  3.16  Fast  Spinning  Algorithm  Roll  Estimating  Error  No  GPS 
Notice  that  there  is  absolutely  no  difference  between  the  algorithm  performance  with¬ 
out  GPS  in  Figure  3.16  and  the  algorithm  performance  with  GPS  shown  back  in  Fig¬ 
ure  3.4  .  This  with  simply  constant  estimates  for  the  gravity  turn  of  the  shell!  The  fast 
spinning  algorithm  only  uses  the  phase  information  between  the  y  and  the  z  axis  rate 
gyro  amplitudes,  not  the  value  of  the  amplitudes  themselves.  The  shell  pitch  over  rate 
Q  and  the  shell  yaw  rate  R  will  affect  the  observation  noise  as  described  later  in 
Section  5.2. 1.1;  otherwise,  the  roll  estimating  algorithm  performance  is  unaffected.  In 
conclusion,  this  roll  estimating  routine  is  really  an  inertial  only  algorithm  and  has  no 
need  for  GPS  measurements. 
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Chapter  4 


Estimating  the  Roll  Angle  of  a  Slowly 
Spinning  Shell 

The  approach  presented  in  Chapter  3  will  only  work  if  the  shell  is  spinning  because 
some  oscillatory  measurements  are  required  to  estimate  the  phase  of  the  roll  angle. 
Besides  the  non-spinning  case,  at  very  small  roll  rates  the  batch  processing  time 
employed  by  the  “fast”  spinning  routine  would  have  to  be  extremely  long  to  include 
enough  of  the  roll  angle  period  to  get  a  decent  phase  angle  estimate.  Clearly,  another 
method  is  required  for  the  slowly  spinning  shell  case. 

The  material  in  this  chapter  will  develop  a  different  approach  for  the  slowly  spinning 
shell  case.  The  first  section  will  show  how  to  use  the  pure  lateral  rate  gyro  measure¬ 
ments  to  estimate  the  roll  angle  of  a  non-spinning  shell.  Note  that  the  problem  must  be 
solved  without  prior  knowledge  of  the  shell  roll  state.  This  method  is  different  from 
the  fast  spinning  method  of  Chapter  3  because  preprocessing  of  the  noisy  rate  gyro 
measurements  is  not  required.  This  solution  will  resemble  that  of  Chapter  3  because  a 
similar  deterministic  linear  least  squares  curve  fitting  method  is  used  to  deal  with  the 
rate  gyro  noise. 

The  slowly  spinning  problem  is  solved  next  in  this  chapter  by  simply  taking  the  non¬ 
spinning  solution  and  trying  it  in  a  simulation  with  a  small  roll  rate.  The  errors  that 
result  will  be  discussed  and  corrected  to  complete  the  derivation  of  the  slowly  spin- 
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ning  shell  roll  estimating  technique.  The  rest  of  the  chapter  is  devoted  to  analysis  of 
the  new  estimator  performance  in  the  presence  of  different  rate  gyro  errors. 

4.1  Solving  the  Non-Spinning  Shell  Problem 

If  the  shell  roll  rate  is  negligible  then  the  shell  roll  attitude  is  fixed  at  some  unknown 
angle.  Under  these  circumstances  the  job  of  extracting  the  roll  angle  from  the  angular 
rates  measured  in  the  body  axis  resembles  the  work  done  earlier  for  the  spinning  shell, 
with  the  exception  that  no  sinusoidal  oscillation  of  the  x  and  y  body  rate  gyro  data  will 
be  present.  EQ  3. 1 . 1  and  EQ  3. 1 .2  as  first  introduced  in  Chapter  3  (repeated  below) 
represent  the  relationships  necessary  to  extract  the  roll  information  from  the  body  axis 
measured  pitch  and  yaw  rates. 

(iiy=  Qcost}) +  i?sin(j)  EQ  (4.1.1) 

co^^=  R  cos  (j)  -  (2  sin  (|)  EQ  (4. 1 .2) 

We  saw  in  the  last  sections  that  Q,  R,  cOy’’,  and  cOz*’  will  be  noisy  measurements.  In  the 
non-spinning  case  the  roll  angle  is  constant  and  we  have  an  independent  set  of  linear 
equations  in  sin(j)  and  cos^)  that  are  not  time  varying. 

4.1.1  Deterministic  Linear  Least  Squares  Roll  Angle  Estimation  for 
the  Non-Spinning  Shell 

EQ  4. 1. 1  and  EQ  4.1.2  can  be  written  in  matrix  form  as  shown  below  for  samples 
taken  at  discrete  time  intervals  tj,  t2  .... 


52 


Qi 

Ql  ^2 
63  ^3 


COS(j) 

sin(|) 


(0 


zl 


b 


R,  -Q, 
Ri  ~Q2 
R3  ~^3 


COS<j) 

sin<{)_ 


R„  -Q, 


EQ  (4.1. 1.1) 


EQ  (4. 1.1. 2) 


cOyj'’  is  the  y  body  axis  rate  gyro  output 

(oj^  is  the  z  body  axis  rate  gyro  output 
Qj  is  the  shell  pitch  over  rate  determined  from  GPS 
Rj  is  the  shell  yaw  rate  determined  from  GPS 
^  is  the  shell  roll  angle 


These  equations  are  in  the  form. 


I’«»l  =  EQ(4.U.3) 

SO  that  the  linear  least  squares  estimate  of  a  is  given  by  the  solution  to  the  normal 
equations  [8]  (repeated  below  from  Chapter  3).  However,  linear  least  squares  estima¬ 
tion  of 


^LLSE  ~  '  ^ 


Z^F 


EQ(4.1.1.4) 


53 


EQ  4.1.1  and  EQ  4. 1.2  separately  (as  set  up  in  equations  EQ  4.1. 1.1  andEQ4.1.1.2 ) 

produces  large  errors  because  the  2x2  matrix  Z^Z  in  the  normal  equations  is  ill-condi¬ 
tioned.  This  occurs  because  the  non-spinning  pitch  rate  “Q”  is  much  larger  than  the 
non-spinning  yaw  rate  “R.”  Inverting  the  ill-conditioned  matrix  (very  large  eigenval¬ 
ues)  is  nearly  dividing  by  zero  which  magnifies  estimation  error.  Since  EQ  4.1.1  and 
EQ  4.1.2  are  linearly  independent,  coupling  data  from  both  equations  into  one  long  Z 

matrix  results  in  a  properly  conditioned  Z^Z  matrix  allowing  the  algorithm  to  yield 
decent  results.  The  final  linear  least  squares  roll  estimating  algorithm  takes  the  follow¬ 
ing  form. 


b 

r  n 

b 

Qx 

b 

^2  ^2 

Q3  ^3 

b 

CO 

b 

-ei 

b 

^2  “^3 

b 

^3  “^3 

. 

K  -Qn 

b 

cost]) 

l_sin(t)J 


EQ(4.1.1.5) 


COyi  is  the  y  body  axis  rate  gyro  output 

is  the  z  body  axis  rate  gyro  output 
Qj  is  the  shell  pitch  over  rate  determined  from  GPS 
Rj  is  the  shell  yaw  rate  determined  from  GPS 
4)  is  the  shell  roll  angle 
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or... 


^nxl 


~  ^nx2  ■  ^2x\ 


EQ(4.1.1.6) 


where... 


®LL5£  “  ^  ^ 


zj-'z". 


EQ(4.1.1.7) 


4.1.2  Performance  of  the  Linear  Least  Squares  Non-Spinning  Shell 
Roll  Estimation  Algorithm 


A  non-spinning  shell  trajectory  was  simulated  at  lOOOHz  and  the  non-spinning  shell 
algorithm  developed  above  was  coded  into  the  Ada  ballistic  artillery  shell  simulation. 
The  batch  processing  data  quantization  time  for  these  results  was  5  seconds  (or  5000 
data  points  at  lOOOHz  collected  in  Y  and  Z  for  LLSE  estimation).  The  roll  error  from 
this  simulation  in  the  absence  of  any  sensor  errors  is  given  below  in  Figure  4. 1  . 


FIGURE  4.1  Non-Spinning  Shell  Roll  Error  (No  Sensor  Errors) 


55 


The  simulation  was  rerun  with  the  same  parameters  and  micro  gyro  noise  as  specified 
in  Chapter  6  and  GPS  velocity  noise  of  a  =  0.1  m/s.  The  results  are  shown  in  Figure 
4.2. 


FIGURE  4.2  Non-Spinning  Shell  Roll  Error  (Noise  Active) 

4.2  Development  of  the  Roll  Estimating  Algorithm  when  the 
Shell  Spin  Rate  is  Small 

4.2.1  Trying  the  Non-Spinning  Algorithm  When  the  Shell  Spin  Rate 
is  Small 

The  non-spinning  shell  formulas  derived  above  apply  over  some  finite  time  interval  At 
if  the  shell  roll  rate  P  is  assumed  to  be  0.  The  size  of  At  and  P  will  dictate  the  amount 
of  error  introduced  by  this  assumption.  If  the  shell  spin  rate  is  small  the  interval  should 
be  sufficiently  large  to  provide  decent  estimates  of  the  shell  body  axis  pitch  and  yaw 
rates  and  yet  be  small  enough  to  keep  error  due  to  the  real  roll  rate  acceptable.  A  pre¬ 
liminary  slow  spinning  roll  estimating  algorithm  emerges  by  first  quantizing  the  shell 
trajectory  into  small  time  steps  At  where  P  is  assumed  to  be  0.  The  algorithm  was  first 
tested  with  a  shell  spin  rate  of  10  degrees  per  second  (0.02778Hz)  and  an  initial  shell 
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roll  angle  equal  45  degrees.  The  system  rate  was  lOOHz  and  the  time  interval  was  1 
second.  This  results  in  the  number  of  observations  per  roll  estimate  N  equal  100.  The 
test  did  not  include  any  sensor  errors.  The  roll  error  from  the  algorithm  is  given  in  Fig¬ 
ure  4.3  . 


FIGURE  4.3  Slow  Spin  Rate  RoU  Error  At  =  1  sec 
At  the  end  of  each  time  step  the  roll  angle  estimate  is  in  error  by  5  degrees.  This  5 
degree  error  is  roughly  constant  throughout  the  trajectory  and  will  be  referred  to  as 
“bias”  for  the  next  few  paragraphs  (note:  it  looks  like  Bias  =  Spin  Rate  *  At/2).  From 
the  initial  5  degree  bias,  the  roll  error  increases  linearly  at  the  roll  rate  (10  deg/sec) 
across  the  time  interval.  This  produces  a  ramp  error  that  grows  from  the  initial  value  of 
5  degrees  to  a  final  value  of  15  degrees  over  the  one  second  interval.  Another  case  was 
tested  with  the  same  trajectory  and  the  time  interval  reduced  to  0.2  seconds.  At  lOOHz 
the  number  of  observations  N  per  roll  estimate  was  20  for  this  case.  Figure  4.4  shows 
the  bias  error  reduced  to  about  2  degrees. 
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Time  in  seconds 

FIGURE  4.4  Slow  Spin  Rate  Roll  Error  At  =  0.2  sec 
The  algorithm  accuracy  improves  as  At  gets  smaller.  However,  as  At  gets  smaller  the 
algorithm  becomes  much  more  susceptible  to  noise.  As  will  be  shown  a  little  later,  the 
large  amount  of  noise  in  the  micro  gyros  will  require  At’s  greater  than  4  or  5  seconds 
for  reasonable  results.  Clearly,  such  a  large  At  would  introduce  unacceptably  large  bias 
and  roll  rate  errors. 

4.2.2  Bias  and  Roll  Rate  Error  Estimation  for  the  Non-Spinning  Shell 
Roll  Algorithm 

The  linear  least  squares  estimation  fits  a  line  through  numerous  data  points  to  estimate 
sin<j)  and  cos(})  assuming  the  roll  rate  is  zero  during  the  batch  processing  time.  Since 
roll  rate  is  not  zero  for  the  slowly  spinning  shell  case,  the  bias  introduced  by  this 
assumption  should  be  a  function  of  the  batch  processing  time  and  the  actual  shell  roll 
rate.  This  is  indeed  the  case.  Observing  the  error  bias  for  several  simulation  runs  pro- 
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duced  the  following  formula  (hinted  at  earlier)  for  estimating  the  bias  of  the  non-spin¬ 
ning  algorithm  roll  error  when  operating  in  a  slowly  spinning  trajectory. 

P  ■  Ar 

Bias  =  EQ(4.2.2.1) 

P  =  Shell  Spin  Rate 

During  the  time  to  collect  the  next  batch  of  data,  the  non-spinning  algorithm  assumes 
the  roll  angle  is  constant.  The  “saw  tooth”  effect  in  Figure  4.3  is  the  roll  rate  dynamic 
error  building  upon  the  already  existing  bias  error  because  the  shell  is  spinning  slowly. 
This  error  can  be  eliminated  through  a  two  fold  procedure.  First,  estimation  of  the 

shell  roll  rate  ((Ox*^)  from  the  x  body  axis  rate  gyro  using  the  Maximum  Likelihood 
technique  from  Appendix  B  and  repeated  below. 

N 

b 

X  ® 

EQ(4.2.2.2) 

Second,  during  the  batch  processing  period,  the  roll  rate  estimate  is  used  to  increment 
the  roll  angle  estimate  from  the  LLSE  value  by  the  amount  PAt  each  discrete  time  step 
of  the  system.  This  relationship  is  given  below. 


=  ^LLSE  +  •  (N  ■  At)  EQ  (4.2.2.3) 

N  =  Number  of  time  steps  since  last  LLSE  estimate 

4.2.3  Performance  of  the  Final  Slow  Spinning  Shell  Roll  Estimating 
Algorithm 

Incorporating  EQ  4.2.2. 1  ,  EQ  4.2.2.2  ,  and  EQ  4.2.2.3  into  the  non-spinning  shell 
algorithm  of  Section  4.1  to  remove  the  bias  and  roll  rate  dynamic  errors  yields  the  final 
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form  of  the  slow  spinning  roll  angle  estimation  routine.  The  ballistic  artillery  shell 
simulation  was  run  with  a  shell  spinning  at  15  degree/sec  (0.04167Hz).  The  system 
rate  was  lOOOHz  and  the  batch  processing  time  for  the  slow  spinning  roll  estimation 
algorithm  was  5  seconds.  The  slow  spinning  algorithm  performance  without  sensor 
errors  is  shown  in  Figure  4.5  .The  simulation  was  rerun  with  the  same  parameters  and 
micro  gyro  noise  as  specified  in  Chapter  6  and  GPS  velocity  noise  of  g  =  0. 1  m/s.  The 
results  are  shown  in  Figure  4.6  . 


FIGURE  4.5  Final  Slow  Spin  Algorithm  Performance, 
P=0.04167Hz,  System  Rate  =  lOOOHz,  (No  Sensor  Errors) 
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FIGURE  4,6  Final  Slow  Spin  Algorithm  Performance  with  Sensor  Noise 
On,  P=0.04167Hz,  System  Rate  =  lOOOHz 

4.3  Selection  of  Batch  Processing  Time  for  the  Slow  Spin 
Roll  Estimating  Algorithm 

Several  batch  processing  intervals  “At”  were  tested  and  the  roll  angle  estimation  per¬ 
formance  was  collected  for  each.  The  algorithm  simulation  incorporated  the  previous 
shell  trajectory  with  shell  spin  rate  P  =  0.04167Hz.  The  system  rate  was  lOOOHz  and 
the  micro  gyro  and  GPS  noise  were  active.  The  performance  of  4  different  LLSE  batch 
processing  At’s  is  shown  in  Figure  4.7  . 
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Batch  DT  =  1  sec 


Batch  DT  =  2.5  sec 


Batch  DT  =  5  sec  Batch  DT  =  1 0  sec 


FIGURE  4.7  Slow  Spin  RoU  Estimation  Algorithm  Performance  for  Differing  Batch 

Processing  Times  (At) 

Observation  of  the  four  examples  shown  suggests  the  choice  of  5  seconds  for  the  slow 
spinning  algorithm  batch  processing  At  to  produce  the  most  accurate  roll  estimate. 


4.4  Finding  the  Shell  Roll  Rate  Crossover  Between  the  Fast 
and  Slow  Spinning  Roll  Estimation  Algorithms 

The  ballistic  artillery  shell  simulation  was  coded  to  perform  either  the  slow  or  the  fast 
spinning  roll  estimating  algorithms.  The  simulated  micro  gyro  noise  and  GPS  velocity 
noise  was  active  for  several  test  cases  consisting  of  shell  spin  rates  between  0  and  2 
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Hz.  The  goal  of  this  exercise  was  to  find  the  shell  roll  rate  where  the  performance  of 
both  algorithms  was  roughly  equal.  This  crossover  point  would  be  used  in  the  overall 
estimator  to  select  the  appropriate  algorithm  depending  on  measured  shell  spin  rate. 
Figure  4.9  shows  the  performance  of  both  the  slow  spin  and  the  fast  spin  algorithms 
side  by  side  for  the  range  of  spin  frequencies  tested.  This  information  is  collected  and 
present  below  in  Figure  4.8  .  The  standard  deviation  and  mean  of  each  roll  error  plot 
shown  in  Figure  4.8  was  calculated  starting  when  the  algorithm  yields  its  first  roll  esti¬ 
mate.  The  absolute  value  of  the  bias  and  standard  deviation  were  added  to  determine 
the  performance  crossover  point. 


FIGURE  4.8  Fast  and  Slow  Spin  Roll  Estimation  Algorithm  Performance  Crossover  Point 
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The  fast  spinning  algorithm  is  more  robust  in  frequency  than  the  slow  spinning  rou¬ 
tine.  However,  as  the  frequency  approaches  OHz  the  fast  spinning  algorithm  develops 
a  significant  bias.  The  slow  spinning  algorithm  is  ineffective  at  roll  rates  where  the 
period  of  the  shell  roll  angle  is  less  than  the  batch  processing  time  At  =  5  seconds.  The 
performance  cross  over  point  is  near  0.08Hz.  This  frequency  represents  the  worst  case 
performance  of  the  filter  incorporating  both  algorithms. 

4.5  The  Slow  Spinning  Shell  Algorithm  without  GPS 

How  does  the  slow  spinning  algorithm  perform  without  GPS  during  the  entire  flight? 
The  simulation  was  run  with  0.04Hz  shell  spin  rate  and  with  the  shell  pitch  over  rate 
estimate  “Q”  equal  to  a  constant  -1  deg/sec  and  the  shell  yaw  rate  estimate  “R”  equal 
to  a  constant  -0.01  deg/sec.  This  was  done  for  the  fast  spinning  shell  case  in 
Section  3.3.  The  roll  estimation  error  for  this  case  in  given  below  in  Figure  4.10  . 


FIGURE  4.10  Slow  Spinning  Algorithm  Roll  Estimation  Error  with  No  GPS 
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The  standard  deviation  of  the  no  GPS  roll  estimation  error  is  3.9075deg.  The  roll  esti¬ 
mation  error  with  GPS  shown  in  Figure  4.6  had  a  standard  deviation  of  4.6919deg. 
This  is  good  news  because,  like  the  fast  spinning  shell  algorithm,  roll  angle  estimation 
does  not  need  to  wait  for  GPS  measurements  in  the  slow  spinning  case.  In  conclusion, 
the  final  roll  estimating  routine  is  an  inertial  only  algorithm  and  has  no  need  for  GPS 
measurements. 
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Chapter  5 

Kalman  Filter  Approach  to  Roll  Angle 
Estimation 


The  goal  of  this  chapter  is  to  derive  two  Kalman  filter  implementations.  I  begin  with 
an  overview  of  Kalman  filtering  to  familiarize  the  reader  with  the  notation  used 
throughout.  The  first  Kalman  filter  will  estimate  the  sine  and  cosine  of  the  phase  of  the 
roll  angle  first  introduced  in  Chapter  3  as  the  solution  the  “fast”  spinning  problem. 
This  will  be  called  the  “fast  spinning  Kalman  filter”.  The  state  space  formulation  of 
the  Chapter  3  routine,  the  derivation  of  the  associated  error  statistics,  and  analysis  of 
the  filter  performance  complete  the  first  major  section  in  the  chapter. 

The  second  part  of  the  chapter  develops  a  Kalman  filter  using  the  measurements  and 
states  introduced  in  Chapter  4  in  solving  the  “slow”  spinning  problem.  This  will  be 
called  the  “slow  spinning  Kalman  filter”.  The  state  space  formulation  of  the  Chapter  4 
routine,  the  derivation  of  the  associated  error  statistics,  and  analysis  of  the  filter  perfor¬ 
mance  complete  the  second  major  section  in  the  chapter. 

Finally,  the  fast  and  slow  spinning  Kalman  filter’s  performances  are  compared  at  roll 
rates  of  OHz,  0.25Hz  and  2Hz.  Some  interesting  insights  can  be  drawn  which  lead  to 
the  concluding  chapter  of  the  thesis. 
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5.1  Kalman  Filter  System  Overview 

The  preceding  sections  have  shown  that  the  gyro  measurements  contain  valuable  atti¬ 
tude  state  information  buried  beneath  noise  and  a  predictable  bias.  Since  the  noise 
variance  and  bias  can  be  characterized  by  understanding  micro  gyro  behavior  in  differ¬ 
ing  dynamics  and  temperatures,  this  information  can  be  used  to  remove  the  errors  and 
uncover  accurate  and  useful  angular  state  information. 

The  Kalman  filter  is  a  recursive  algorithm  that  provides  a  minimum  variance  estimate 
of  the  state  errors  through  optimal  use  of  the  error  characteristics.  White  noise  of  some 
variance  is  processed  for  each  state  in  the  Kalman  filter.  The  discrete  Kalman  filter 
computes  a  state  dynamics  matrix  relating  the  vehicle  state  vector  x  at  times  t+1  and  t. 
This  matrix  is  found  by  taking  advantage  of  the  state  space  relationship... 

=  F(t)  -xit)  +B(t)  •m(0  +G(t)  wit)  EQ (5.1.1) 

The  finite  difference  equation  relating  discrete  samples  in  the  vector  x  at  times  t+1  and 
t  in  the  absence  of  the  forcing  function  is  then... 

x(r+ 1)  =  A(r)  •x(r)  +vt;  EQ  (5.1.2) 

The  matrix  A  is  a  function  of  time  and  is  updated  every  time  step  accordingly.  The 

observations  available  from  sensors  are  related  to  the  state  vector  x  by  the  following... 

y(0  =  C(0  -  xit)  +v  EQ (5.1.3) 

The  minimum  error  covariance  linear  least  squares  estimate  for  the  state  vector  x 

based  on  the  observations  y  is  then  calculated  by  the  following  recursive  formulas... 
5(0  =  «(?|f-l)  +!P(tlt-l)C(t)^[c(t)iP(tl!-l)C(t)^+wT\y(0  -C(05(qf-1))  EQ  (5.1.4) 


68 


nt)  =  -!P('|'-l)C(0^[c(f)2’(rK-l)C(/)^+3^(0]~’c((05’(«|?-l))  EQ(5.1.5) 

The  error  covariances  of  the  state  vector  “fP’  and  the  noise  sequence  v,  along  with  the 

strength  of  the  white  noise  w  are  as  follows... 


X  =  x-x 

T  = 

£■[^(0  •w(r)T  =  (lit)  -dCr-T) 


EQ  (5.1.6) 
EQ  (5.1.7) 
EQ  (5.1.8) 
EQ  (5.1.9) 


The  state  vector  x  and  the  error  covariance  matrix  f0[t)  are  propagated  every  time  step 
using  A(t)  and  the  covariance  matrix  of  the  discrete  noise  vector  w,  denoted  as  (J.  The 
propagation  equations  are  as  follows. 


x(r+l|0  =Ait)-xit)  EQ  (5.1.10) 

fP(r+l|r)  =  A(r)  •  fp(0  •A(0^  +  G(0  •  0,(0  •  G(f)^  eq(5.i.ii) 

A  complete  treatment  of  Kalman  filtering  can  be  found  in  reference  [12] .  This  refer¬ 
ence  was  the  source  of  the  information  presented  in  this  section  and  the  notation  from 
this  reference  was  used  throughout.  (Note:  t  =  to,  tQ+l...,  where  1  is  the  discrete  time 
increment,  not  necessarily  1  second)  A  block  diagram  of  this  filter  is  given  in  Figure 
5.1  . 
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Kaiman  Filter 


Propagation 

x(t+llt)  =  A(t)x(t) 

2(t+llt)  =  A(t)S[t)A(t)'’‘'  +  GQP'^ 


Measurement  Update 
x+  =  x  +  yc’’{C2C’^+3g-‘{y-Cx-) 
^  =  ‘F  -  rFC’^{C5C’^+J^‘‘Cy 

where 

x'^  =  x(t)  2^=ff(t) 

x'  =  x(tlt-l)  5^  =  i(tlt-l) 


y  =  vector  of  observations 

from  data, 

[CO  b  ®  b  0)  d 


A 


X 


CO  b 

X 

CO  b 

y 

COb 

z 


MIMU  (Micro  Gyro) 


FIGURE  5.1  Kalman  Filter  Implementation 

5.2  Kalman  Filter  Implementation  of  Fast  Spinning 
Algorithm 

5.2.1  State  Space  Realization  for  Algorithm 

A  five  state  Kalman  Filter  was  developed  for  the  fast  spinning  shell  case  to  provide  the 
iterative  optimal  estimate  of  cosOp  and  sinOp  originally  introduced  in  Section  3.2  on 

page  35  for  the  fast  spinning  LLSE  batch  processing  technique.  The  other  three  states 
used  include  the  shell  roll  rate  “P”,  and  the  y  and  z  body  axis  lumped  equivalent  rate 
gyro  biases,  “Bias_y”  and  “Bias_z”.  “Lumped  equivalent  rate  gyro  biases”  are  bias 
errors  in  the  rate  gyro  angular  rate  measurements  due  to  some  combination  of  rate 
gyro  non-orthogonality  error,  scale  factor  error  and  bias  stability  error.  The  dynamics 
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of  each  state  are  modelled  as  a  constant  plus  noise  as  shown  in  EQ  5.2. 1.1  .  Note:  no 
process  noise  “Q,"  will  be  used  in  the  filter. 


X  (f+  1) 


1  0  0  0  0 
0  10  0  0 
0  0  10  0 
0  0  0  1  0 
0  0  0  0  1 


■  x(t)  +  vv 


EQ  (5.2. 1.1) 


The  observations  of  the  gyro  biases  come  from  preprocessing  the  y  and  z  body  axis 
rate  gyro  data.  This  preprocessing  simply  calculates  the  recursive  average  of  the  gyro 


signals  shown  below  in  EQ  5.2. 1 .2  and  EQ  5.2. 1 .3  to  use  as  measurements  in  the  filter. 


YGyroLumpedBiaSof^^^^^^^^^  =  EQ(5.2.1.2) 

b 

0) 

ZGyroLumpedBias  Observation  =  EQ(5.2.1.3) 

(Dy*^  is  the  y  body  axis  rate  gyro  output 

is  the  z  body  axis  rate  gyro  output 
n  is  the  number  of  discrete  time  steps  elapsed 


The  relation  between  the  rate  gyro  lumped  equivalent  bias  and  the  observations  of  the 
states  cosd>p  and  sinOp  is  shown  below.  Because  these  four  states  all  depend  on  the  y 
and  z  axis  rate  gyro  outputs,  the  measurements  are  correlated  with  each  other.  This 
correlation  decrease  with  time.  The  measurement  covariance  matrix  is  derived  in 
Section  5.2. 1.1. 
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EQ(5.2.1.4) 


sin<j)(0  =  — - — 

Q+R 

siiK})  (t)  =  sin  (P  ■  t)  ■  cos<E)p  +  cos  (P  ■  t)  ■  sin^>p 
which  leads  to  the  following  for  sin<t)(t)... 


EQ  (5.2. 1.5) 


sin<t>(0  =  (i^bias 


(  ^  1 

-CO.. 

bias 

f  *2  "I 

2  2 

2  2 

\Q  +R  J 

le  +p  J 

+  sin  (P  ■  t)  ■  cos0p  +  cos  (P  •  /)  •  sin®^  EQ  (5.2.1.6) 


and  the  similar  relationship  for  cos<|)(t)  is... 


Q(xi\R(o’' 
cos^(t)  =  — ^ 

Q+R 


EQ  (5.2. 1.7) 


cos<l)(f)  =  a  bias  ■ 


Q  ] 

+  CO.  .  ^  • 
bias 

^  ^  ) 

2  2 

2  2 

\Q  +R  J 

Ifi  +p  J 

+  cos  (P  ■  0  •  C0S<I>^-  sin  (P  ■  t)  ■  sinO^  EQ  (5.2.1.8) 


The  final  state  space  realization  for  the  fast  spinning  shell  case  Kalman  Filter  is  sum¬ 
marized  below. 


co„ 


b 

S— 

^  n 

b 

I  — 

^  n 
sin(j)  (r) 
[costj)  (r)J 


1  0 
0  1 
0  0 
R 


0 


0 

0 

1 

-Q 


0 

0 

0 


0 

0 

0 


2  2  2  2 
Q+R  Q+R 


sin  (P  •  t)  cos  (P  •  t) 


0  —p—i  ^  2 
Q+R  Q+R 


1 

P 

' 

^bias 

COS  Op 

sin  Op 

+  vEQ(5.2.1.9) 


where... 


(£)y^  is  the  X  body  axis  rate  gyro  output 
Z0y’^/n  is  the  average  y  body  axis  rate  gyro  output 

E©2^/n  is  the  average  z  body  axis  rate  gyro  output 
sin(l)(t)  is  the  measurement  created  via  EQ  3.1.2 
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cos<|)(t)  is  the  measurement  created  via  EQ  3.2.1 
P  is  the  shell  roll  rate  estimate 
Q  is  the  average  shell  pitch  over  rate  determined  priori 
R  is  the  average  shell  yaw  rate  determined  priori 

®bias^  is  the  estimate  of  the  lumped  average  y  body  rate  gyro  bias 

(Obias^  is  the  estimate  of  the  lumped  average  z  body  rate  gyro  bias 
is  the  phase  of  the  shell  roll  angle  from  EQ  3.2.3 

which  is  of  the  form... 

y(0  =  C(0  -^(0  +y  EQ  (5.2.1.10) 

and 


1  0  0  0  0 
0  10  0  0 

x(t+l)  =  0  0  1  0  0-^(0+w  EQ(5.2.1.11) 

0  0  0  1  0 
0  0  0  0  1 

which  is  of  the  form... 

x(r+l)  =A(t)-x(t)+w  EQ  (5.2.1.12) 

5.2.1.1  Error  Modelling  of  Observation  Noise 

The  covariance  matrix  of  the  observation  noise  ‘X’  is  shown  in  EQ  5.2. 1.1.1  .Notice 
that  ‘X’  is  nearly  singular  when  t/At  equals  one.  This  problem  is  avoided  by  starting 
the  filter  when  t/At  equals  two. 
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The  elements  of  the  covariance  matrix  are  derived  below.  To  begin,  the  micro  gyro 
angle  random  walk  of  9.2  deg/it-hr  yields  the  rate  error  variance  as  described  in 
EQ  5.2.1. 1.2. 


_ f,,  *^1  rfoo  n-rad  y  Ihour  1000  nnn-r,^^  rad  /r  ^  i  i 

J  =  3555J  —  =  0.007162.-^  EQ(5.2.1.1.2) 


{  b\  f  [  b 

cov^co^  J  =  cov^to^  J  =  covi  co^ 


EQ  (5.2. 1.1. 3) 


Note  that  the  noise  for  the  bias  state  observations  is  time  varying  as  shown  in 
EQ  5.2.1. 1.4. 


—  J  =  -  .cov(Xto„)  =  — ^ 


EQ  (5.2. 1.1. 4) 


The  relationship  between  the  covariance  of  the  sin(l)(t)  and  cos(t)(t)  (observations  4  and 
5)  and  the  shell  pitch  over  rate  “Q”  and  yaw  rate  “R”  is  developed  below. 


£'[sin4)(t)  •sin<|)(t)]  =  E  — - — ^ 

A  Q^  +  R  A 


=  ^(4,4)  EQ  (5.2. 1.1. 5) 


rr  6(o/+rco/y 

£[cos(t)(Q  ■cos<j)(r)]  =  E  — - - 

Q^  +  R  K 


1^(5, 5)  EQ(5.2.1.1.6) 


m4)  = 


2  2 

Q  +R 


(  0  V  r 

^(5,5)  =  El 

Ic  +R  ) 


b  b- 


V 

]4- 

ll 


2  2 

Q  +R 


■  jE'[co/  •  CO^^]  EQ  (5.2. 1.1. 7) 


R  Y 
2  2 
Q  +R  J 


£’[«/•  to/]  EQ(5.2.1.1.8) 


The  covariance  of  sin(t)(t)  and  cos(j)(t)  will  change  with  time  in  magnitude  if  Q  and  R 
vary.  Note  that  when  both  Q  and  R  are  small  in  magnitude,  the  covariance  can  become 
very  large.  The  simulation  is  run  using  constant  Q  =  -0.017453  rad/sec  and  R  =  - 


0.0017453  rad/sec  which  results  in  the  following... 
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%iA,A)  =  0.321812  - Var(^a)y*j  + 3218.12  •  Vflr(^Co/J  =  23.05  EQ(5.2.1.1.9) 

^{5,5)  =  3218.12  •yar[c0y*j  + 0.321812 =  23.05  EQ (5.2.1.1.10) 
Note:  23.05  is  a  large  value  for  the  variance  of  the  error  in  the  value  of  sine  and  cosine. 
This  large  number  is  due  to  a  combination  of  the  constants  Q  and  R  and  the  large  rate 
gyro  noise.  Assuming  the  y  and  z  axis  rate  gyro  signals  are  corrupted  by  independent 
zero  mean  white  noise  processes,  the  following  relationships  are  true  for  the  cross 
covariance  of  sin(j)(t)  and  cos<{)(t). 

©/]  =  0  EQ  (5.2.1.1.11) 

£(cos4.(/)  -sirK^lO]  =  .v^{co/)-  •Var((o/)  =  0  EQ (5.2.1.1.12) 

[q^*r  J  [q^+r  ] 

The  cross  correlation  between  observations  sin(j)(t)  and  cos(})(t)  are 

developed  below. 


EQ  (5.2.1.1.13) 


The  relationship  in  EQ  5.2.1.1.14  is  true  because  the  rate  gyro  measurements  at  the 


present  time  are  uncorrelated  with  all  the  rate  gyro  measurements  in  the  past. 


eL  ‘ .  y ^1  .  .  eL  ‘  ^  y “t-l  .  Efa...:.i^. 

y  ^  n  n  z  n  n 


EQ  (5.2.1.1.14) 


Which  leads  to... 


n 


EQ  (5.2.1.1.16) 


5.2. 1.2  Initial  State  and  Initial  Error  Covariance  Matrix 

To  initialize  the  Kalman  Filter,  an  initial  guess  at  each  state  variable  is  required  “xq”, 
and  the  error  covariance  matrix  associated  with  the  initial  guess  or  “fPo”.  The  first  three 
slates  are  readily  initialized  by  the  first  value  of  the  appropriate  micro  gyro  output.  The 
corresponding  initial  error  covariance  is  simply  the  covariance  of  the  micro  gyro  out¬ 
put  in  “1^1,1)”.  Since  cosOp  and  sin<I)p  are  unknown,  an  initial  value  d>p  =  0  deg  is 
assumed.  The  initial  error  covariance  for  cos4>p  and  sinOp  is  found  by  first  assuming 
<I>P  is  distributed  uniformly  between  0  and  2n.  The  expected  values  of  both  cos3>p  and 
sin<I>p  are  derived  in  EQ  5.2. 1 .2. 1  and  EQ  5.2. 1 .2.2  and  represents  the  initial  error 
covariance  for  these  states. 
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sin$  (t)  =  sin(P  •  t  +  <I>p) 
cos$  (t)  =  cos  (P  ■  t  +  Op) 

=  atanfM^) 

''  cos<|)  (0  ^ 


EQ  (5.2. 1.2.5) 
EQ  (5.2. 1.2.6) 

EQ  (5.2. 1.2.7) 


5.2.2  Kalman  Filter  Performance 

This  Kalman  Filter  was  coded  into  the  Ada  simulation  for  Monte  Carlo  analysis.  The 
trajectory  modelled  is  the  same  30Km  155mm  howitzer  simulation  described  in 
Section  6.3  with  a  2Hz  shell  roll  rate.  GPS  measurements  were  not  used  for  any  of 
these  simulations.  The  shell  pitch  over  rate  “Q”  was  set  to  a  constant  value  of  -1  deg/ 
sec  and  the  shell  yaw  rate  “R”  was  set  to  a  constant  value  of  -0.01  deg/sec.  The  Kal- 
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man  Filter  performance  in  relation  to  the  modelled  micro  gyro  errors  specified  in 
Section  6.1  is  presented  below. 

5.2.2.1  Performance  in  Noise  Only 

The  Kalman  Filter  roll  error  in  the  noise  only  environment  is  given  in  Figure  5.2  .  The 
y  and  z  axis  rate  gyro  non-orthogonality  was  also  active  to  demonstrate  the  filters  abil¬ 
ity  to  estimate  and  remove  bias.  Roll  error  standard  deviation  for  this  case  was  a  mere 
0.7098  degrees. 


FIGURE  5.2  Fast  Spin  Kalman  Filter  Roll  Error  Noise  Only  Case 
The  performance  of  the  individual  states  in  the  Kalman  Filter  are  given  in  Figure  5.3  , 
Figure  5.4  ,  Figure  5.5  ,  Figure  5.6  ,  and  Figure  5.7  .  The  phase  estimate  from  states 
four  and  five  is  provided  in  Figure  5.8  . 
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FIGURE  5.6  CosOp  Estimate  from  Kalman  Filter 


FIGURE  5.7  SinOp  Estimate  from  Kalman  FOter 


FIGURE  5.8  Phase  Estimate  from  ArcTan  of  States  4  and  5 
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5.2.2.2  Performance  in  Noise  and  Lateral  Rate  Gyro  Errors 
Similar  to  the  fast  spinning  LLSE  algorithm  of  Section  3.2,  the  roll  estimation  process 
is  not  sensitive  to  the  accuracy  of  the  y  and  z  body  axis  rate  gyro  measurement  accura¬ 
cies.  As  long  as  the  relative  amplitudes  of  the  lateral  gyros  are  significantly  different, 
the  phase  angle  estimation  uses  the  rate  of  change  of  amplitude  of  each  signal,  not  the 
signal  value  itself.  This  technique  allows  for  a  very  robust  routine  in  terms  of  lateral 
rate  gyro  errors.  Figure  5.9  shows  the  simulation  results  with  all  the  y  and  z  rate  gyro 
errors  active  at  once  (i.e.  bias,  g  sensitivity,  noise  on  all  three  gyros,  scale  factor  and 
non-orthogonality).  No  significant  degradation  in  performance  results. 


Time  in  seconds 


FIGURE  5.9  Roll  Error  with  Y  and  Z  Body  Axis  Rate  Gyro  Errors  All  Active 
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5.2.2.3  Performance  in  X  Axis  Rate  Gyro  Errors 

Figure  5. 10  shows  that  x  axis  rate  gyro  bias,  g  sensitivity,  and  noise  on  all  three  gyros 
have  little  affect  on  algorithm  performance. 
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FIGURE  5.10  Roll  Error  with  X  Axis  Bias,  G  Sensitivity,  and  Noise 
When  the  simulation  is  rerun  with  x  axis  scale  factor  error  only,  the  roll  estimation  per¬ 
formance  is  degraded  as  shown  in  Figure  5.11.  Note  the  slope  of  the  roll  error  curve  is 
exactly  1.44  deg/sec  or  the  amount  of  error  introduced  into  “P”  (the  shell  roll  rate  esti¬ 
mate)  by  the  x  axis  rate  gyro  2000ppm  scale  factor  error  at  2Hz.  The  filter  is  very  sen¬ 
sitive  to  the  estimated  value  of  the  roll  rate  “P”.  Referring  back  to  EQ  5.2. 1.7  ,  recall 
that  P  is  used  in  the  matrix  C(t)  to  relate  both  the  sin<l>p  and  cosOp  states  to  their 

respective  measurements.  The  Kalman  filter  implementation  integrates  the  roll  angle 
estimate  error  introduced  by  x  axis  scale  factor.  This  error  could  be  substantially 
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reduced  by  adding  states  to  the  navigation  filter  to  estimate  x  axis  scale  factor  error 
from  accelerometer  information  and  GPS  as  discussed  briefly  in  Section  3.2.4 


FIGURE  5.11  Roll  Error  with  X  Axis  Scale  Factor 

5.2.2.4  Performance  Summary  for  Fast  Spinning  Roll  Estimation  Kalman  Filter 

Total  system  performance  was  demonstrated  by  first  simulating  the  shell  at  a  spin  rate 
of  2Hz  with  all  the  errors  active,  and  re-running  the  simulation  with  all  errors  except 
the  X  axis  rate  gyro  scale  factor.  The  Kalman  Filter  developed  in  this  section  provides 
roll  error  accuracies  with  a  standard  deviation  of  0.8777  deg  in  the  case  where  x  axis 
scale  factor  error  is  not  included  (Figure  5.12  ).  This  is  in  comparison  to  a  standard 
deviation  of  4  degrees  for  the  LLSE  fast  spinning  algorithm  under  the  same  condi¬ 
tions.  If  X  axis  scale  factor  is  not  removed,  the  resulting  roll  error  performance  is  also 
shown  in  Figure  5.12  and  results  in  prohibitive  roll  estimation  errors. 
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Xsf  Inactive  Xsf  Active 


FIGURE  5.12  Complete  Roll  Error  Performance  for  2Hz  Kalman  Filter 
with  and  without  X  Axis  Scale  Factor  Error 

5.3  Kalman  Filter  Implementation  of  Slow  Spinning 
Algorithm 

5.3.1  State  Space  Realization  for  Algorithm 

The  equations  developed  for  the  slow  spinning  shell  algorithm  in  Section  4.2  are 
repeated  here  for  the  development  of  the  slow  spinning  Kalman  filter.  In  the  slow  spin¬ 
ning  case,  cos(|)  and  sin(])  can  be  estimated  directly  from  the  lateral  gyro  data. 

(Siy=  Qcos^  +  Rsin^  EQ  (5.3.1. l) 

=  i?cos(j)-2sin(t)  EQ(5.3.1.2) 

A  three  state  Kalman  filter  is  developed  using  the  rate  gyro  signals  about  three  axis  as 
observations  of  the  three  state  realization  where  the  first  state  is  the  shell  roll  rate  “P”, 
the  second  is  coscj),  and  the  third  is  sint])  (EQ  5. 3. 1.3  ).  The  lumped  lateral  rate  gyro 
biases  are  not  included  as  states  because  they  are  inconsequential  at  small  roll  rates. 
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where... 


1  0  0 
QQ  R 
p  R  -Q 


P 

COS<]) 

sin  (I) 


EQ  (5.3. 1.3) 


is  the  X  body  axis  rate  gyro  output 
©y*’  is  the  y  body  axis  rate  gyro  output 

©2*^  is  the  z  body  axis  rate  gyro  output 
(j)  is  the  shell  roll  angle  at  the  current  time 
P  is  the  shell  roll  rate  estimate 

Q  is  the  average  shell  pitch  over  rate  determined  priori 
R  is  the  average  shell  yaw  rate  determined  priori 

This  is  of  the  form... 

y(t)  =  C(t)  -X(t)  +y  EQ(5.3.1.4) 

The  state  vector  is  time  varying  because  of  the  small  roll  rate  P.  This  information  is 
incorporated  into  the  prediction  portion  of  the  state  space  realization  as  developed 
below. 

cos  (4)  (r  +  Ar) )  =  cos  (<|)  (0 +^Ar) 
sin  ((j)  (r  + Ar) )  =  sin  (t)  +  PAt) 
coscj)  (t  +  At)  =  cos<l)  (t)  ■  cos  (PAt)  -  sin(|)  (t)  ■  sin  (PAt) 
sincj)  (r  +  Af)  =  sin<})  (r)  •  cos  (PAt)  +  cos<t)  (t)  ■  sin  (PAt) 
which  becomes... 

1  0  0 
^(^+1)=  0  cos  (PAr)  -sin  (PAr)  ■x(t)+w  EQ(5.3.1.9) 

0  sin  (PAr)  cos  (PAr) 


EQ  (5.3. 1.5) 
EQ  (5.3. 1.6) 
EQ  (5.3. 1.7) 
EQ  (5.3.1. 8) 
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where  P  is  the  shell  roll  rate  estimate.  At  is  the  discrete  time  interval  and  EQ  5.3. 1.9  is 
of  the  form... 


.x(/+l)  =  A  (t)  -xit)  +w  EQ (5.3.1.10) 

This  state  space  realization  is  used  in  combination  with  the  discrete  Kalman  Filtering 
equations  to  provide  an  recursive  roll  angle  estimate  every  discrete  time  step  by  taking 
the  arctangent  of  states  2  and  3  outside  of  the  Kalman  Filter. 

^  ‘  EQ(5.3,1.U) 

5.3.1.1  Error  Modelling  of  Observation  Noise 

The  observation  covariance  matrix  ‘X’  for  the  slow  spinning  case  is  simply  the  covari¬ 
ance  of  the  three  axis  micro  rate  gyro  outputs.  These  measurements  have  uncorrelated 
noise  in  their  outputs  with  variance  as  already  derived  in  EQ  5.2. 1.1  . 


'll 


EQ  (5.3.1. 1.1) 


EQ  (5.3.1. 1.2) 


0.007162  0  0 

0  0.007162  0 

0  0  0.007162 


EQ  (5.3. 1.1. 3) 


5.3.1.2  Initial  State  and  Initial  Error  Covariance  Matrix 

The  filter  is  initialized  with  an  initial  estimate  of  the  state  vector  Xq  and  the  error  cova¬ 
riance  of  the  initial  state  estimate  Tq.  The  initial  state  estimate  of  the  roll  rate  is  simply 
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the  pure  x  axis  rate  gyro  measurement  at  to  and  the  corresponding  error  covariance  of 

this  estimate  is  Var(®x*^)-  Guessing  the  roll  angle  to  be  0  degrees  provides  and  initial 
estimate  of  cosine  and  sine  of  roll  to  be  1  and  0  respectively.  The  error  covariance  of 
these  initial  estimate  is  the  same  as  previously  derived  in  Section  5.2. 1.2  on  page  76. 


Xq  = 

(^o) 

1 

zz 

COS<j)Q 

0 

1 

00 

3‘ 

-e- 

0.007162  0  0 

0  1.5  0 

0  0  0.5 


EQ(5.3.1.2.1) 


EQ  (5.3.1.2.2) 


5.3.2  Kalman  Filter  Performance 

This  Kalman  Filter  was  coded  into  the  Ada  simulation  for  Monte  Carlo  analysis.  The 
trajectory  modelled  is  the  same  30Km  155mm  howitzer  simulation  described  in 
Section  6.3  with  a  0.04Hz  shell  roll  rate.  GPS  measurements  were  not  used  for  any  of 
these  simulations.  The  shell  pitch  over  rate  “Q”  was  set  to  a  constant  value  of  -1  deg/ 
sec  and  the  shell  yaw  rate  “R”  was  set  to  a  constant  value  of  -0.01  deg/sec.  The  Kal¬ 
man  Filter  performance  in  relation  to  the  modelled  micro  gyro  errors  specified  in 
Section  6.1  is  presented  below. 
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5.3.2.1  Performance  in  Noise  Only 

Figure  5.13  presents  the  roll  error  from  the  slow  spinning  Kalman  Filter  with  micro 
gyro  noise  the  only  error  source  active. 


FIGURE  5.13  Kalman  Filter  Performance  in  Noise  at  0.04Hz 
The  performance  of  the  individual  states  in  the  Kalman  Filter  is  shown  in  Figure  5. 14 , 
Figure  5.15  ,  and  Figure  5.16  .  Notice  that  the  estimates  of  sin(j)(t)  and  cos({>(t)  grow 
outside  their  expected  range  of  between  plus  and  minus  one.  They  could  have  been 
constrained  in  an  non-linear  manner  so  that  the  sum  of  the  squares  is  held  equal  to  one. 


FIGURE  5.14  Roll  Rate  “P”  Estimate  From  Kalman  Filter 
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FIGURE  5*16  Sm({>  Estimate  from  Kalman  Filter 

53.2.2  Performance  with  X,  Y,  and  Z  Axis  Rate  Gyro  Bias  Active 

Figure  5.17  shows  the  simulated  filter  roll  error  performance  when  bias  errors  are 

activated  on  all  three  axis  rate  gyros  as  well  as  gyro  noise. 
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FIGURE  5.17  RoU  Error  Performance  in  Noise  with  X,  Y,  and  Z  Axis  Rate  Bias  Active 

5.3.2.3  Performance  with  X,  Y,  and  Z  Axis  Rate  Gyro  Scale  Factor  Active 
Figure  5.18  shows  the  simulated  filter  roll  error  performance  when  scale  factor  errors 
are  activated  on  all  three  axis  rate  gyros  as  well  as  gyro  noise. 


FIGURE  5.18  Roll  Error  Performance  in  Noise  with  X,  Y,  and  Z  Scale  Factor  Errors  Active 

5.3.2.4  Performance  with  X,  Y,  and  Z  Axis  Rate  Gyro  G-Sensitivity  Active 

Figure  5.19  shows  the  simulated  filter  roll  error  performance  when  g-sensitivity  errors 
are  activated  on  all  three  axis  rate  gyros  as  well  as  gyro  noise. 
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FIGURE  5.19  Roll  Error  Performance  in  Noise  with  X,  Y,  and  Z  Axis  G  Sensitivity  Active 
Figure  5.20  shows  the  resulting  performance  with  only  the  x  axis  g  sensitivity  active. 
The  deceleration  due  to  the  aerodynamic  drag  causes  a  large  g-load  along  the  x  axis 
which  decreases  as  the  shell  decelerates  throughout  the  trajectory  and  results  in  the 
degradation  in  filter  performance.  The  x  axis  g  sensitivity  error  results  in  a  bias  error 
of  -4.4859  degrees. 
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FIGURE  5.20  Roll  Error  Performance  in  Noise  with  only  the  X  Axis  G  Sensitivity  Active 
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5.3.2.5  Non-Orthogonalities  and  Misalignment  Errors 

The  simulated  trajectory  was  run  with  y  and  z  axis  non-orthogonality  active  to  demon¬ 
strate  the  effects  of  physical  misalignment  of  the  gyros.  The  y  axis  was  misaligned  96 
mrad  (5.5  degrees)  in  the  yz  plane  and  the  z  axis  was  likewise  misaligned  96  mrad  in 
the  yz  plane.  Figure  5.21  shows  the  resulting  roll  error  performance.  The  large  errors 
introduced  by  the  simulated  misalignment  emphasize  the  need  for  precision  calibra¬ 
tion  upon  MIMU  assembly  and  installation  in  the  shell  to  reduce  misalignment  type 
errors  to  much  less  than  96  mrad. 


FIGURE  5.21  Roll  Error  Performance  in  Noise  with  Y  and  Z  Axis 
Rate  Gyro  Non-Orthogonality  of  96  mrad 

5.3.2.6  Performance  Summary  for  Slow  Spinning  Roll  Estimation  Kalman  Filter 

Total  system  performance  was  demonstrated  by  simulating  the  shell  at  a  spin  rate  of 
0.04Hz  with  all  the  errors  active  (except  non-orthogonality).  The  Kalman  Filter  devel¬ 
oped  in  this  section  provides  roll  error  accuracies  with  a  standard  deviation  of  2.1962 
deg  and  a  mean  of  -1.8227  deg  in  this  case  (Figure  5.22  ).  This  is  in  comparison  to  a 
standard  deviation  of  8.4252  degrees  and  a  mean  of  -2.248  degrees  for  the  LLSE  slow 
spinning  algorithm  under  the  same  conditions. 
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FIGURE  5.22  Roll  Error  Performance  of  Slow  Spinning  Kalman  Filter  with  all  Errors  Active 

5.4  Performance  Crossover  Point  Between  the  Fast  and  Slow 
Spinning  Kalman  Filters 

The  analysis  of  Section  4.4  on  page  62  is  repeated  for  the  two  Kalman  filters  by  simu¬ 
lating  both  routines  (with  all  micro  gyro  errors  active)  at  different  roll  rates  and  com¬ 
paring  the  resulting  roll  error  performance. 

5.4.1  Comparing  Filter  Performance  at  Small  Roll  Rates 

The  slow  spinning  Kalman  filter  was  much  more  robust  in  frequency  than  the  slow 
spinning  LLSE  algorithm  of  Section  4.2.  The  performance  crossover  point  is  difficult 
to  quantify  exactly  by  simply  observing  the  roll  error  performance,  but  crude  compari¬ 
sons  yield  very  similar  roll  error  performance  for  both  filters  at  about  0.25Hz  shell  roll 
rate  as  shown  in  Figure  5.23  . 
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Slow  Spin  Kalman  Filter  Fast  Spin  Kalman  Filter 


FIGURE  5.23  Roll  Error  Performance  for  both  Kalman  Filters  at  0.25Hz  Roll  Rate 

5.4.2  Performance  when  the  Shell  Spin  Rate  is  OHz 

Roll  error  performance  gets  worse  when  the  shell  is  not  spinning  at  all.  This  is  no  sur¬ 
prise  for  the  fast  spinning  Kalman  filter  of  Section  5.2  because  the  existence  of  a  shell 
spin  rate  was  fundamental  to  the  development  of  the  state  space  realization  for  the  fast 
spinning  case.  The  roll  error  performance  of  the  fast  spinning  Kalman  filter  when  the 
shell  spin  rate  is  OHz  and  all  micro  gyro  errors  are  active  is  shown  in  Figure  5.24  . 
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FIGURE  5.24  Fast  Spinning  Kalman  Fflter  Roll  Error  for  Non  Spinning  Case  (OHz) 
The  slow  spinning  Kalman  filter  state  space  realization  was  derived  from  the  slow 
spinning  LLSE  algorithm  of  Section  4.2  on  page  56  which  began  by  solving  the  non 
spinning  shell  case  and  then  correcting  the  errors  incurred  when  the  shell  spin  rate  is 
small.  However,  the  performance  of  the  slow  spinning  Kalman  filter  in  the  non  spin¬ 
ning  shell  case  as  shown  below  in  Figure  25  is  much  worse  than  the  roll  error  perfor¬ 
mance  under  the  same  conditions  for  a  slowly  spinning  case  of  0.04Hz  from  Figure 
5.22. 


FIGURE  5.25  Slow  Spinning  Kalman  FOter  Roll  Error  for  Non  Spinning  Case  (OHz) 
What  is  the  cause  of  the  slow  spinning  Kalman  filter  performance  degradation  in  the 
non  spinning  case?  The  simulation  was  run  several  times  isolating  the  different  micro 
gyro  errors  to  observe  their  individual  impact.  All  these  runs  were  similar  to  the 
slowly  spinning  errors  discussed  in  Section  5.3  except  for  the  y  and  z  axis  (lateral  rate 
gyros)  bias  error  shown  below  in  Figure  5.26  . 
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FIGURE  5.26  Slow  Spinning  Kalman  Filter  Roll  Error  with  Y  and  Z  axis  Rate  Gyro  Bias 
Active  and  Noise  with  a  Shell  Roll  Rate  of  OHz 

The  state  space  realization  developed  in  Section  5.3.1  uses  the  y  and  z  axis  rate  gyro 
signals  as  direct  observations  of  sine  and  cosine  of  the  roll  angle.  When  these  signals 
are  biased,  the  resulting  estimates  of  sine  and  cosine  of  the  roll  angle  are  biased  as 
well.  The  arctangent  of  these  biased  estimates  results  in  the  10  degree  roll  error  bias 
shown  in  Figure  5.26  .  When  the  shell  is  spinning  slowly,  the  errors  are  modulated  by 
the  roll  motion  and  tend  to  cancel,  which  explains  the  improved  performance  at 
0.04Hz.  In  conclusion,  a  small  roll  rate  near  0.04Hz  produces  the  best  performance  for 
either  Kalman  filter. 

5.4.3  Performance  of  the  Slow  Spinning  Kalman  Filter  at  2Hz 

The  slow  spinning  LLSE  batch  processing  algorithm  produced  useless  estimates  at  roll 
rates  greater  than  0.1 5Hz  because  the  formulation  of  the  batch  processing  technique 
did  not  adequately  account  for  roll  rate  dynamics.  However,  the  slow  spinning  Kalman 
filter  does  account  for  these  dynamics  in  the  prediction  step  by  including  the  shell  roll 
rate  estimate  in  the  transition  matrix  A(t)  to  form  the  state  space  model.  Given  this,  the 
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results  shown  in  Figure  5.27  for  the  slow  spinning  Kalman  filter  performance  at  the 


“fast”  2Hz  shell  spin  rate  are  not  surprising. 


Xsf  Inactive  Active 


FIGURE  5.27  Slow  Spinning  Kalman  FQter  Performance  with  Shell  Roll  Rate  of  2Hz 
In  conclusion,  both  the  fast  and  slow  spinning  Kalman  filters  have  similar  perfor¬ 
mance  characteristics  at  roll  rates  from  0.25Hz  to  2Hz.  When  the  shell  is  not  spinning, 
the  slow  spinning  Kalman  filter  does  a  much  better  job  of  finding  the  fixed  roll  angle 
than  the  fast  spinning  Kalman  filter. 
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Chapter  6 

Computer  Simulation  Development 


6.1  Simulation  Overview 

A  high  fidelity  Monte  Carlo  simulation  of  a  generic  artillery  shell  equipped  with  an 
MIMU  and  a  24  channel  GPS  receiver  was  developed  by  modifying  the  Embedded 
GPS  Doppler  Test  Facility  software  model  of  the  lJH-60  Blackhawk  Helicopter  docu¬ 
mented  in  [10] .  The  simulation  is  written  in  Ada  to  run  on  a  Sun  Microsystems  work¬ 
station.  The  modifications  to  the  existing  GPS/Doppler  simulation  are  summarized  as 
follows... 

•  Rename  and  modify  variables  to  define  specific  artillery  shell  characteristics 

•  Eliminate  unnecessary  sensor  models  including  doppler  navigation  models,  gyro 
magnetic  compass  models,  barometric  altimeter  models,  and  vertical  gyro  models 

•  Add  MIMU  software  model  of  micro  gyro  and  micro  accelerometer  triads  with 
appropriate  errors 

•  Adapt  GPS  model  to  shell  platform  by  updating  antenna  installation  and  gain 
objects 

•  Add  state  estimation  objects  for  performance  analysis  of  roil  estimation  algorithms 
developed  for  this  thesis 

•  Add  variables  to  the  timeliner  script  to  specify  artillery  shell  physical  characteris¬ 
tics,  sensor  error  characteristics,  algorithm  performance  variables,  and  sensor 
installation  locations 

The  simulation  architecture  is  depicted  in  Figure  6.1  . 
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FIGURE  6.1  Monte  Carlo  Simulation  Overview 
The  inertial  Ada  Artillery  Shell  Simulation  reads  data  from  a  trajectory  file  in  Earth 
Centered  Earth  Fixed  (ECEF)  coordinates.  This  data  consists  of  time,  position,  veloc¬ 
ity,  acceleration,  specific  force,  attitude  quaternion,  angular  rate,  and  angular  accelera¬ 
tion.  This  trajectory  file  is  created  by  another  Ada  simulation  described  in  Section  6.3. 
Operating  at  a  specified  system  rate  (typically  lOOOHz),  the  Artillery  Shell  Simulation 
reads  in  the  truth  data  for  the  shell  center  of  mass,  determines  the  truth  data  for  each 
sensor  location,  exercises  each  sensor  model  with  the  error  characteristics  specified  in 
the  timeliner  script,  exercises  state  estimation  objects  which  use  the  modelled  sensor 
data,  and  finally  collects  and  reports  the  truth  and  measured  parameters  to  an  output 
file.  The  output  file  is  reduced  via  a  Matlab  routine  for  observation  of  the  system  per¬ 
formance. 


100 


6.2  Navigation  Sensor  Performance 

6.2.1  Micro  Mechanical  Rate  Gyro  Errors 

Table  1  records  the  current  state  of  technology  for  Draper  micro  gyro  accuracy  [3] . 


TABLE  1.  Micro  Gyro  Specifications 


Stability  (deg/hr) 

480.0 

Scale  Fact  (ppm) 

2000.0 

G-Sens  (deg/hr/g) 

720.0 

Misalign  (mrad) 

96.0 

Rand  Walk  (deg/rt-hr) 

9.2 

This  section  will  describe  the  error  mechanisms  present  in  the  micro  gyros.  This  infor¬ 
mation  is  not  clearly  documented  in  any  single  source,  however  many  of  the  terms  are 
described  in  reference  [7] . 

•  Stability  Error 

Sometimes  denoted  as  “drift”,  rate  gyro  angular  output  wanders  away  from  truth  at  the 
RMS  rate  denoted  as  “stability  error”  in  Table  1  .  Since  the  time  constant  is  large 
(between  1  hour  and  10  hours)  for  stability  error,  this  effect  has  been  modelled  as  a 
constant  bias  for  the  artillery  shell  simulation  of  an  80  second  trajectory. 

•  Misalignment  Error 

The  job  of  the  navigation  system  is  to  determine  the  angular  attitude  of  the  vehicle  in 
inertial  space.  The  sensors  can  only  detect  the  angular  orientation  of  the  sensor.  Mis¬ 
alignment  error  deals  with  the  imprecision  in  mounting  the  assembled  MIMU  unit 
onto  the  vehicle.  Misalignment  is  the  difference  between  the  actual  orientation  of  the 
equipments  sensitive  axis  and  the  intended  orientation  of  that  axis.  This  is  a  constant 
angular  error  and  is  normally  kept  small  by  precision  mounting  and  measuring  tech¬ 
niques  during  installation. 
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Non-Orthogonality  Error 


Non-orthogonality  error  refers  to  imprecision  in  assembling  the  MIMU  unit  itself.  The 
MIMU  consists  of  three  micro  gyros  and  the  intent  is  to  mount  them  perfectly  along  3 
orthogonal  axes.  Non-orthogonality  error  results  when  one  gyro  input  axis  leans  into 
the  plane  containing  the  remaining  two  gyro  input  axes.  This  non-orthogonal  gyro  will 
detect  components  of  the  angular  rates  about  the  other  axes.  Figure  6.2  shows  the  rela¬ 
tionship  between  the  unit  vectors  in  the  Navigation  Body  (NB)  frame  and  a  unit  vector 
corresponding  to  the  misaligned  z  axis  gyro.  The  error  due  to  non-orthogonality  in  the 
z  axis  is  found  by  determining  the  components  of  each  NB  frame  angular  rate  sensed 
by  the  misaligned  rate  gyro.  This  relationship  is  given  in  EQ  6.2.1 . 


FIGURE  6.2  Non-Orthogonality  Error  Depiction 
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Scale  Factor  Error 


The  micro  gyro  scale  factor  error  is  calculated  as  a  percentage  of  the  true  angular  rate 
sensed  by  the  rate  gyro.  Scale  factors  produce  an  error  in  the  measured  angular  rate  of 
a  magnitude  that  is  proportional  to  the  true  angular  rate  being  measured.  Scale  factor 
errors  are  most  troublesome  at  high  angular  rates. 

^^^^^scalefactor  =  ScaleFuctor  ■  EQ  (6.2.2) 

•  G  Sensitivity  Error 

“Sensitivity  is  a  change  in  output  to  a  change  in  an  undesirable  or  secondary  input.”[7] 
Gyroscope  output  variation  due  to  accelerations  incident  on  the  instrument  are  called 
g-sensitivity  error.  This  produces  rate  bias  error  proportional  to  the  amount  of  specific 
force  in  a  maneuver  and  lasting  the  duration  of  the  specific  force.  Note:  specific  force 
is  equal  to  the  inertial  acceleration  of  a  body  minus  gravitational  acceleration. 

Error ^  EQ  (6.2.3) 


•  Angle  Random  Walk 

Micro  gyroscopes  are  noisy  sensors.  Figure  6.3  is  actual  bench  test  data  from  Draper 
laboratories  showing  the  noise  level  of  the  micro  gyro.  The  micro  gyro  acts  as  an  inte¬ 
grator  of  the  sensed  angular  rate.  The  actual  sensor  output  of  A0  integrates  the  noise 
shown  in  Figure  6.3  to  produce  a  smoother  signal  that  randomly  wanders  through  a 
certain  maximum  error.  Angle  random  walk  is  modelled  in  the  Monte  Carlo  simula- 
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tion  by  generating  white  noise  at  the  system  data  rate  and  multiplying  by  the  appropri¬ 
ate  constant  to  result  in  the  specified  random  walk  characteristics. 


Time  (5  second  intervals) 


FIGURE  6.3  Gyro  Output  From  Static  Bench  Test 
•  Temperature  Sensitivity 

The  micro  gyro  is  a  good  thermometer.  Temperature  bias  of  the  micro  gyro  is  also  evi 
dent  in  the  bench  test  data  shown  in  Figure  6.4  .  Temperature  bias  will  have  to  be 
removed  by  the  navigation  algorithm  to  preclude  large  errors  in  the  integrated  solu¬ 
tion. 


Time  (5  second  intervals)  .j  q-* 

FIGURE  6.4  Gyro  Output  vs.  Temperature  from  Bench  Test 


6.2.2  Micro  Mechanical  Accelerometer  Errors 


Table  1  records  the  current  state  of  technology  for  Draper  micro  accelerometer  accu¬ 
racy  [3] . 


TABLE  2.  Micro  Accelerometer  Specifications 


Stability  (millig) 

58.0 

Scale  Fact  (ppm) 

4000.0 

G^-Sens  (microg/g^) 

8000.0 

Misalign  (mrad) 

11.0 

Velocity  Random  Walk 
(cm/s/rt-hr) 

340.0 

The  error  terms  have  similar  definitions  whether  they  represent  micro  accelerometer 
errors  or  micro  gyro  errors.  This  section  will  describe  the  error  mechanisms  present  in 
the  micro  accelerometers  not  explained  earlier  for  the  micro  gyros. 

•  Sensitivity  Error 

Accelerometer  output  variation  due  to  the  square  of  the  acceleration  incident  on  the 

instrument’s  sensitive  axis  is  called  g^  sensitivity  error.  This  produces  acceleration 
bias  error  proportional  to  the  amount  of  specific  force  squared  in  a  maneuver  which 
lasts  the  duration  of  the  specific  force. 

Error =  s]ensiti.>ity  •  sensed  EQ  (6.2.1) 


•  Velocity  Random  Walk 

Micro  accelerometers  are  noisy  sensors.  The  micro  accelerometer  acts  as  an  integrator 
of  the  measured  specific  force.  The  actual  sensor  output  of  AV  measurements  inte¬ 
grates  the  noise  to  produce  a  velocity  signal  that  randomly  wanders  through  a  certain 
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maximum  error.  Velocity  random  walk  is  modelled  in  the  Monte  Carlo  simulation  by 
generating  white  noise  at  the  system  data  rate  and  multiplying  by  the  appropriate  con¬ 
stant  to  result  in  the  specified  random  walk  characteristics. 

6.2.3  GPS  Velocity  Measurements 

The  Guided  Munition  Demonstration  Program  autopilot  navigation  filter  will  be 
restricted  to  use  only  the  position  and  velocity  measurements  from  the  embedded  GPS 
receiver’s  own  navigation  filter.  The  follow-on  program  will  incorporate  psuedorange 
and  delta  range  measurements  directly  from  the  GPS  receiver  into  a  single  navigation 
filter  for  a  more  optimal  navigation  solution.  The  Ada  GPS  simulation  only  models 
GPS  psuedorange  and  delta  range  from  each  satelhte,  not  position  and  velocity  for  a 
specific  GPS  receiver.  Because  GPS  velocity  measurements  were  required  for  the 
analysis  in  Chapter  2 ,  GPS  velocity  was  crudely  modelled  by  adding  velocity  noise  (a 
=  0.  Im/s)  to  the  true  velocity  from  the  trajectory  file. 

6.3  Trajectory  Generation 

The  trajectory  generating  software  was  written  in  Ada  to  create  a  data  file  of  variables 
that  described  a  ballistic  artillery  shell  trajectory  in  ECEF  coordinates. 

6.3.1  Artillery  Shell  Dynamics  Modelling 

The  artillery  shell  was  modelled  as  a  point  mass  with  a  drag  coefficient  C^o-  The  posi¬ 
tion  and  velocity  of  the  shell  were  determined  by  integrating  Newton’s  law  of  motion 
for  a  point  mass.  The  point  mass  is  influenced  by  the  force  of  gravity,  a  drag  force  act¬ 
ing  directly  opposite  the  shell  velocity  vector,  the  Coriolis  force  due  to  the  shell  veloc- 
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ity  in  the  rotating  ECEF  reference  frame,  and  the  rotational  force  due  to  the  rotation  of 
the  earth.  [6]  A  spinning  shell  will  also  experience  an  additional  yawing  moment  due 
to  the  interaction  of  the  spinning  shell  boundary  layer  with  the  atmosphere  [5] .  This 
effect  is  called  the  yaw  of  repose  and  is  ignored  by  the  trajectory  generator. 


‘"shell  =  +  C£>0P''r.i(^)  +  2  '  («>£  ®  ^rel)  +  ®  (®£  ®  ^rw)  EQ  (6.3.1) 

where... 


m  =  mass 

Qdo  =  Drag  Coefficient 

Vfgi  =  Shell  Velocity  Vector  Relative  to  Earth’s  Surface 
(£>p  =  Earth  Rotation  Rate 

Prei  =  Shell  Position  Vector  Relative  to  Earth ’s  Surface 
p  =  Density  of  Air 


The  initial  velocity  Vq  is  known  and  acts  in  the  direction  of  the  azimuth  and  elevation 
of  the  gun  barrel.  The  initial  position  Pq  is  the  latitude  and  longitude  of  the  gun’s  loca¬ 
tion.  The  program  terminates  when  the  position  vector  in  the  North  East  Down  (NED) 
frame  is  positive  in  the  “z”  or  “down”  direction. 

The  trajectory  generator  assumes  the  shell  nose  or  roll  axis  is  perfectly  aligned  with 
the  shell  velocity  vector  at  all  times.  The  pitch  and  yaw  attitudes  are  simply  deter¬ 
mined  by  calculating  the  angles  defining  the  velocity  vector  in  the  NED  frame.  The 
angular  rates  in  pitch  and  yaw  are  calculated  by  differentiating  the  change  in  attitude 
across  the  time  step  At.  The  shell  roll  rate  and  initial  roll  angle  are  constant  numbers 
selected  prior  to  program  execution.  The  roil  angle  is  derived  by  integrating  the  roll 
rate  throughout  the  trajectory  from  the  initial  roll  angle. 
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6.3.2  Artillery  Shell  Simulation  Parameters 

A  single  trajectory  was  modelled  for  all  the  analysis  presented  in  this  thesis.  The  shell 
roll  rate  along  this  single  trajectory  was  varied  between  0-2Hz  to  produce  the  variety 
of  results  discussed  herein  (note:  the  trajectory  generator  only  considers  constant  shell 
roll  rates).  Table  3  below  documents  the  parameters  (taken  from  [3] )  that  define  the 
trajectory  simulated  and  recorded  in  the  trajectory  files  used  by  the  Ada  artillery  shell 
simulation. 


TABLE  3.  Trajectory  Simulation  Parameters  Used  for  Analysis 


Trajectory  Parameter 

Value  Used 

Shell  Diameter 

155  mm 

Cdo 

0.0033 

Initial  Velocity 

830  m/sec 

Initial  Latitude 

40  deg  north 

Initial  Longitude 

70  deg  west 

Gun  Barrel  Azimuth 

0  deg  (due  north) 

Gun  Barrel  Elevation 

38.75  deg 

The  preceding  parameters  produce  an  artillery  shell  trajectory  that  travels  30  km  down 
range  in  about  79  seconds.  The  following  set  of  figures  show  the  details  of  the  simu¬ 


lated  trajectory. 
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FIGURE  6.5  30km  Simulated  Trajectory  TYne  Position  in  NED 
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Ihie  Attitude  in  Body  Axis  (Roll  Rate  OHz) 
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FIGURE  6.8  30km  Simulated  TVajectory  True  Attitude  Rate  in  Body  Axis  (Roll  Rate  OHz) 
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Chapter  7 

Conclusions  and  Future  Research 


7.1  Conclusions 

The  goal  of  this  thesis  was  to  develop  a  method  of  estimating  the  local  vertical  state  of 
a  ballistic  artillery  shell  spinning  at  a  rate  from  0-2Hz.  The  shell  was  equipped  with  a 
GPS  receiver  providing  position  and  velocity  data  at  IHz  and  an  MIMU  providing  A9 
and  AV  measurements  at  lOOOHz.  In  addition,  any  initial  roll  angle  state  information 
was  considered  lost  due  to  the  high  dynamics  (6200g  and  high  shell  spin  rates) 
incurred  when  launching  an  artillery  shell  out  of  a  cannon. 

A  method  of  estimating  artillery  shell  pitch  over  rate  (due  to  the  gravity  turn)  and  shell 
yaw  rate  (due  to  Coriolis  force)  from  GPS  velocity  measurements  was  developed  and 
its  performance  was  simulated.  This  information  was  combined  with  data  from  the  lat¬ 
eral  micro  rate  gyros  to  produce  two  orthogonal  signals  from  which  the  shell  roll  angle 
could  be  estimated.  Several  attempts  to  estimate  the  roll  angle  of  a  spinning  shell  were 
made  and  their  simulated  performance  was  documented.  The  most  accurate  algorithm 
was  a  deterministic  linear  least  squares  batch  processing  technique  that  estimated  the 
roll  angle  within  -i-  or  -  4  degrees  at  a  shell  spin  rate  of  2Hz  throughout  the  trajectory 
(without  X  axis  scale  factor  errors  included).  Each  micro  gyro  error  modelled  was  indi¬ 
vidually  analyzed  for  performance  degradation  of  the  fast  spinning  algorithm.  Mis¬ 
alignment  errors  and  non-orthogonality  errors  had  a  large  impact  when  a  component 
of  the  roll  rate  was  projected  onto  one  or  both  of  the  lateral  micro  gyros.  This  created  a 
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large  bias  in  the  lateral  rate  gyro  data  that  would  ruin  the  algorithm  performance  if  not 
estimated  and  removed.  Errors  in  the  spin  axis  rate  gyro  due  to  scale  factor,  misalign¬ 
ment,  or  non-orthogonality  also  contributed  a  significant  performance  degradation.  An 
attempt  to  estimate  x  axis  rate  gyro  error  by  Fourier  transform  of  lateral  rate  gyro  data 
was  ineffective.  In  conclusion,  it  was  noted  that  precision  calibration  and  manufactur¬ 
ing  of  the  micro  gyro  would  be  required  to  reduce  non-orthogonality  and  misalign¬ 
ment  errors.  Also,  x  scale  factor  error  would  have  to  be  estimated  and  removed  in  the 
system  navigation  filter  using  GPS  information. 

The  spinning  shell  algorithm  performance  became  degraded  as  the  shell  roll  rate 
approached  0  Hz.  Therefore,  a  slowly  spinning  roll  estimation  algorithm  was  devel¬ 
oped  using  a  modified  deterministic  linear  least  squares  batch  processing  technique. 
The  simulated  performance  of  this  algorithm  provided  a  roll  angle  estimate  within  +  or 
-  7  degrees  throughout  most  of  the  shell  trajectory  at  slow  spin  rates.  The  fast  and  slow 
spinning  roll  estimation  algorithm  performance  crossover  point  was  determined  to  be 
at  a  shell  spin  rate  of  about  0.08Hz. 

The  performance  of  both  algorithms  was  unaffected  when  the  shell  pitch  over  rate  and 
yaw  rate  estimates  from  GPS  were  replaced  with  constant  numbers  indicative  of  the 
average  angular  rates  expected  for  shell  pitch  over  and  yaw.  The  algorithms  did  not 
rely  on  the  value  of  the  lateral  rate  gyro  amplitudes  but  simply  the  change  in  cunplitude 
observed  across  the  roll  period.  Eliminating  the  need  for  GPS  allowed  the  algorithms 
to  be  active  during  the  15  seconds  required  for  GPS  reacquisition  after  launch. 
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Two  extended  Kalman  filter  implementations  (one  for  both  the  fast  and  slow  spinning 
algorithms)  were  developed  and  each  filter’s  performance  was  simulated.  The  fast 
spinning  shell  Kalman  filter  included  five  states  and  yielded  roll  estimates  an  order  of 
magnitude  better  than  the  corresponding  batch  processing  technique.  Each  micro  gyro 
error  was  individually  analyzed  for  performance  impact  on  the  filter.  X  axis  scale  fac¬ 
tor  error  made  the  only  significant  performance  degradation  of  all  the  errors  modelled. 
A  2000ppm  X  scale  factor  error  caused  the  roll  estimate  to  linearly  drift  away  from 
truth  at  a  rate  of  1.44  deg/sec. 

The  slow  spinning  shell  Kalman  filter  included  three  states  and  also  yielded  roll  esti¬ 
mates  an  order  of  magnitude  better  than  the  corresponding  batch  processing  technique 
with  all  simulated  errors  active  and  the  shell  spin  rate  at  0.04Hz.  Each  micro  gyro  error 
was  individually  analyzed  for  performance  impact  on  the  filter.  Non-orthogonality 
error  also  caused  significant  performance  degradation  when  allowed  into  the  slow 
spinning  simulation  and  will  have  to  be  eliminated  by  sensor  calibration.  X  axis  G  sen¬ 
sitivity  error  made  the  only  significant  performance  degradation  of  all  the  errors  mod¬ 
elled.  X  axis  scale  factor  error  was  not  significant  at  small  roll  rates.  Also,  when  the 
shell  trajectory  was  non  spinning  (OHz),  the  performance  was  degraded.  For  the  non 
spinning  case,  the  lateral  rate  gyro  bias  errors  could  not  cancel  each  other  out  and  the 
resulting  roll  angle  was  biased  from  tmth  by  10  degrees  (see  Figure  5.26 ).  Finally,  the 
slow  spinning  Kalman  filter  performance  at  2Hz  was  comparable  to  the  fast  spinning 
Kalman  filter  performance  at  that  rate. 
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In  summary,  extended  Kalman  filter  roll  angle  estimation  of  a  spinning  shell  firom  iner¬ 
tial  sensor  data  can  provide  accurate  performance  for  shell  roll  rates  between  0-2Hz. 
GPS  is  only  required  for  correcting  x  axis  scale  factor  error  in  the  fast  spinning  shell 
case.  Precision  calibration  and  manufacturing  of  the  MIMU  will  be  required  to  reduce 
the  effects  of  misalignment  and  non-orthogonality  errors. 

7.2  Future  Research 

Future  studies  should  integrate  the  extended  Kalman  filter  states  from  this  thesis  into 
the  complete  shell  navigation  filter  and  use  the  additional  information  available  to  esti¬ 
mate  and  remove  x  axis  scale  factor  errors,  g  sensitivity  errors,  misalignment  errors, 
and  bias  errors.  Performance  degradation  due  to  noise  in  the  shell  roll  rate  and  slowly 
varying  roll  rates  should  be  studied  as  well  as  the  effect  of  adding  the  yaw  of  repose 
dynamics  to  the  trajectory  behavior.  Data  quantization  of  the  inertial  micro  sensors 
was  not  an  error  parameter  in  the  Thesis  simulation.  An  adequate  hardware  quantiza¬ 
tion  level  for  the  gyros  needs  to  be  determined.  There  may  be  a  trade  off  between 
quantization  level  and  update  rate. 
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Appendix  A 

Roll  Estimation  For  Low  Noise  Gyros 


Solving  EQ  3.2. 1  and  EQ  3.2.2  for  sincj)  and  coscj)  where  ^  is  time  varying  produces 
the  following  relationships. 


Qo  +i?(o. 

cos(t)(r)  =  — i ^  EQ(A.i) 

2  ^ 

Q  +R 
*  -  <2©/ 

sin(l)(0  =  — - r-^  EQ(A.2) 

2 

Q  +R 

Notice  also  that... 


JcOS<t)^/(t)  = 

.Q(£)\R(x)^ 

sm(})(f)  =  f  ^  rf<t) 

EQ(A.3) 

q\r 

Jsin(j)<i(j)  =  ■ 

COS(l)  (t)  =  \  \  2 

EQ  (A.4) 

Q^  +  R 

=  Pdt 

EQ(A.5) 

where  P  is  the  roll  rate  and  is  considered  constant  across  the  small  interval  d<|).  Inte¬ 
grating  the  noisy  measurements  will  make  extracting  the  roll  information  less  suggest¬ 
ible  to  noise.  The  final  algorithm  uses  estimates  of  yaw  and  pitch  rate  derived  from  the 
GPS  Euler  angle  projection  combined  with  the  rate  gyro  data  from  the  x,  y  and  z  body 
axes.  This  data  is  combined  and  integrated  over  time.  The  arctangent  function  pro¬ 
vides  the  proper  quadrant  for  the  roll  angle  based  on  the  value  of  sin(])  and  cosc]). 
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Time  in  seconds 


FIGURE  A.l  Error  in  Estimated  Roll  Angle  With  Roll  Rate  =  1/4  Hz  and  Data  Rate  =  lOOHz 
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This  algorithm  assumes  we  know  the  body  axis  angular  rates  and  cOx*^-  This 

information  is  provided  by  the  micro  gyros  and  is  extremely  noisy.  Figure  A.2  shows 
the  simulated  gyro  output  with  noise  for  a  trajectory  with  constant  1/4  Hz  roll  rate. 

(O 
CO 
0) 
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Appendix  B 


Estimating  Angular  Rate  from  Noisy  Gyro  Data  Using 
Non-Random  Parameter  Estimation 


The  observed  gyro  output  “y”  is  modelled  as  the  true  angular  rate  “x”  (a  fixed  deter¬ 
ministic  parameter  for  the  non-spinning  case)  plus  zero  mean  white  gausian  noise  “w” 
with  some  fixed  variance  (EQ  B.  1 ).  A  vector  of  observations  of  the  gyro  output  corre¬ 
sponds 


y  =  x  +  w  EQ(B.l) 

to  several  observations  of  the  constant  value  “x”  plus  a  vector  of  noise  observations 

(EQ  B.2 ).  A  maximum  likelihood  estimate  (ML)  of  the  non-random  parameter  “x”  is 


>^1 

Wi 

3^2 

^2 

3^3 

=  JC  + 

^3 

EQ  (B.2) 


obtained  by  finding  the  argument  that  maximizes  the  probability  density  function  for 
Y  given  x  [9] . 


■^Flx  “  n  T  ■  ^ 

where... 


EQ  (B.3) 
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EQ  (B.4) 


m^  =  X 


EQ(B.5) 


so  that... 


^F|a: 


(F-a:) 


2^  -lA/ 

EQ  B.6  is  maximized  when  the  exponent  is  equal  to  zero.  This  yields... 


EQ  (B.6) 


^ML 


1 


EQ  (B.7) 


Solving  for  the  ML  estimate  yields  the  following  relationship  which  means  the  best 
estimate  of  “x”  is  the  average  of  the  values  observed  in  “y”  [9] . 


N 


/V  2=1 

^ML  =  — 


EQ  (B.8) 


If  a  \  eclor  has  N  observations  of  the  shell  spin  rate,  pitch  rate  or  yaw  rate  in  the  body 
axis  from  a  micro  gyro,  the  estimated  angular  rates  of  the  shell  in  the  body  axes  are 
given  by  the  following. 


/  =  I 

(a  x  =  ~ 


Zb 

CO  X.- 


N 


EQ  (B.9) 
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EQ(B.10) 

N 

Xb 

CO  z/ 

c=  1 

N 


EQ  (B.ll) 
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Appendix  C 


Roll  Estimation  by  Locating  Signal  Maximums  and 
Minimums 

Noise  in  the  gyros  completely  corrupts  the  roll  estimation  algorithm  presented  in 
Appendix  A  .  This  is  because  the  arctangent  function  is  indeterminate  when  noisy 
gyro  measurements  are  integrated  and  the  resulting  random  walking  signal  cannot  be 
easily  constrained  within  +  or  -  1.  This  is  illustrated  in  Figure  C.  1  which  shows 
EQ  C.  1  and  EQ  C.2  for  sincj)  and  cos(|)  for  1/4  Hz  roll  rate.  The  gyro  noise  is  included 
and  the  integration  of  the  noise  produces  the  random  walking  signal  shown  below. 


.  .Q(0  +/?© 

Jcos(t>£?(t)  =  sin(j)(r)  =  I — - - - — 

Q^  +  R 

.  .RdiJ’  -  Q(0^ 

I  sin(t)J(])  =  -cos<l)(r)  =  J — - - ^ 

Q^  +  R 


EQ(C.l) 


EQ  (C.2) 


FIGURE  C.l  Accumulated  Sine  and  Cosine  Roll  from  Algorithm 
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A  different  technique  was  implemented  which  involves  estimating  the  maximum  and 
minimum  points  for  both  EQ  C.  1  and  EQ  C.2 .  This  information  provides  t^ax  or  tmin 
which  pinpoints  the  roll  angle  when  <j)  =  0  and  n  for  the  cosine  curve  and  when  (j)  =  nil 
and  371/2  for  the  sine  curve.  Between  the  max  and  min  values  the  roll  angle  is  esti¬ 
mated  using  the  roll  rate  estimate  EQ  C.l  and  EQ  C.2  estimate  roll  from  the 
maximums  and  minimums  coming  from  the  accumulated  cosine  curve  in  equation 
EQ  C.  1 .  A  similar  set  of  equations  is  used  to  estimate  roll  from  the  accumulated  sine 
curve  of  equating  EQ  C.2  . 

♦  (0  =  f  +  0>/-  «-<„„)  EQ(C.1) 

•KO  =  Y  +  ffl/- EQ(C.2) 
Data  quantization  rate  plays  a  big  roll  in  this  algorithm.  Finding  the  maximums  and 
minimums  required  estimating  the  slope  of  the  accumulated  sine  signal  in  Figure  C.l . 
The  noise  evident  near  the  peaks  required  data  smoothing  by  quantization  and  averag¬ 
ing.  The  error  in  the  roll  estimate  is  equal  to  the  error  in  the  estimated  spin  rate  times 
the  error  in  the  estimation  of  tn,ax  or  t^jj^.  Data  quantization  of  20  seconds  at  lOOHz 
(0.2  seconds  At)  yielded  the  tjj,ax  Vin  estimates  shown  in  Figure  C.2  . 
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FIGURE  C.2  Tniav  and  T,„iQ  Estimates  from  Accumulated  Sine 
Since  the  data  quantization  At  was  0.2  seconds  and  the  roll  rate  was  1/4  Hz,  the  maxi¬ 
mum  error  in  from  this  algorithm  should  lie  between  +  or  - 18  degrees  (not  including 
errors  in  estimating  the  spin  rate).  Figure  C.3  shows  the  roll  error  and  confirms  this  is 
indeed  the  accuracy.  Better  accuracy  could  be  achieved  by  increasing  the  data  rate  to 
lOOOHz  and  thus  decreasing  At  to  0.02  seconds.  This  could  potentially  provide  a  fac¬ 
tor  of  10  improvement. 


FIGURE  C.3  Roll  Error  for  Maximum/Minimum  Algorithm  (Data  at  lOOHz) 
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